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A  programming  language  was  developed  to  provide  a  modular  approach  for  specifying 
intelligence  for  a  vision-servoed  fruit-picking  robot.  The  programming  language  consisted  of 
models,  states,  and  global  parameter  definitions.  Models  mapped  sensor  and  system  data  to 
binary  values  according  to  criteria  established  by  a  programmer.  Global  parameters  were  used 
for  tuning  modeling  criteria  on-line  and  for  establishing  communication  between  an  operator 
and  program  execution.  States  specified  robot  actions  and  used  model  results  to  determine  when 
actions  had  been  completed  or  if  problems  existed  that  prevented  completion  of  specified 
actions.  A  state  network  was  created  by  linking  states  together  with  decision  statements.  The 
intelligence  to  perform  a  robot  task  was  programmed  in  the  state  network. 

A  state  network  was  developed  to  provide  a  vision-servoed  robot  with  the  intelligence 
needed  to  automatically  pick  fruit.  The  picking  intelligence  recognized  and  solved  many 
problems  associated  with  grove  operation  of  the  robot.  Grove-related  problems  that  were 
addressed  in  the  state  network  included  attempting  to  pick  fruit  that  were  out  of  the  robot  work 

x 


space;  detecting  excessive  fruit  motion  that  prevented  a  successful  pick  cycle;  detecting  when  a 
fruit  vanished  from  view  during  a  pick  cycle;  picking  fruit  that  were  partially  occluded  by 
limbs  and  leaves;  and  detecting  collisions  between  the  robot  and  tree. 

Over  150  hours  of  development  time  were  spent  with  the  robot  in  grove  settings.  Fruit 
were  successfully  picked  under  a  variety  of  production  conditions.  During  a  35-minute  period  of 
fruit  picking,  321  completed  pick  cycles  were  performed.  This  averaged  to  approximately  6.5 
seconds  per  pick  cycle.  It  was  estimated  that  75%  of  the  fruit  that  the  robot  attempted  to  pick 
were  actually  removed. 
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CHAPTER  1 
INTRODUCTION 


Seasonal  labor  is  used  to  harvest  citrus  crops  throughout  the  world.  Due  to  potential 
worker  shortage  and  increasing  labor  costs,  there  has  been  a  desire  to  mechanize  fruit 
harvesting.  Two  different  approaches  taken  for  mechanically  harvesting  fruit  are  nonselective 
mass  removal  and  selective  singular  removal.  Several  methods  of  mechanical  fruit  removal 
have  been  investigated  (Whitney,  1977).  Of  these  methods,  shakers  received  greater  interest 
and  research.  Shakers  vibrate  tree  structures  using  different  methods  (limb,  trunk,  foliage,  air, 
and  water)  to  remove  fruit.  Although  these  methods  obtained  moderate  to  good  removal 
efficiencies,  disadvantages  included  damage  to  the  tree  and  fruit  crop  and  reduction  of  the 
following  year's  crop. 

Harrell  et  al.  (1985)  used  a  commercial  robot  to  determine  if  singularly  picking  fruit 
could  successfully  be  performed  in  the  laboratory  setting.  Plastic  oranges  were  attached  to  a 
canopy  constructed  from  green  plastic  foliage.  A  black  background  was  used  to  enhance  the 
contrast  between  oranges  and  the  background.  The  robot  was  modified  to  a  revolute—revolute- 
prismatic  configuration.  The  robot  had  horizontally  and  vertically  rotating  revolute  joints  and 
a  prismatic  (sliding)  joint.  A  picking  device  was  incorporated  at  the  end  of  the  prismatic  joint 
for  fruit  detachment.  A  black  and  white  vision  system  was  incorporated  into  the  picking  device 
housing  to  provide  end-of-the-arm  fruit  sensing.  The  offset  of  an  orange's  projection  from  the 
image  center  was  used  to  vision-servo  the  two  revolute  joints  to  establish  alignment  between 
the  fruit  and  the  end-effector.  The  prismatic  joint  was  extended  toward  the  fruit  until  the 
fruit's  image  size  exceeded  pre-established  image  diameters.  Outward  movement  was  halted 
and  the  picking  device  was  activated  to  grasp  the  fruit  stem.  When  the  robot  arm  retracted 
from  the  simulated  canopy,  the  orange  was  detached  from  the  tree.  After  the  arm  was  fully 
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retracted,  the  picking  device  released  the  orange.  This  picking  cycle  was  repeated  as  long  as  a 
large  blob  could  be  detected  in  the  image.  If  an  object  was  not  located  by  the  vision  system,  the 
revolute  joints  moved  the  picking  device  to  a  new  location.  If  an  object  was  located  at  the  new 
location,  the  picking  cycle  was  performed.  The  robot  was  moved  to  a  home  location  and  the 
picking  operation  was  terminated  after  the  robot  had  been  moved  to  all  of  the  established 
search  positions.  Picking  cycle  times  of  3-7  seconds  were  common  in  the  laboratory  setting. 

It  was  recognized  that  many  improvements  would  have  to  be  made  to  successfully  move 
the  technology  to  a  grove  setting.  The  structured  work  environment  of  uniform  size  and  color 
oranges  and  the  uniformly  colored  foliage  poorly  mimicked  actual  structures  found  in  a  grove.  A 
different  imaging  system  would  be  needed  to  locate  oranges  in  natural  settings  due  to  the 
variety  of  background  scenes  encountered  in  groves.  A  method  to  determine  the  distance  from 
the  end-effector  to  an  object  would  be  necessary  to  determine  how  far  the  end-effector  should 
extend  into  the  canopy.  A  robot  for  grove  operation  would  require  a  larger  work  volume,  a  faster 
dynamic  response,  and  an  improved  picking  device.  The  robot  controller  would  require 
improved  task  level  control  to  allow  alterations  to  the  robot  picking  program  to  be  made 
quickly  and  efficiently. 

To  move  the  picking  technology  to  the  grove,  a  three  degree-of-freedom  robot  was 
designed  and  constructed  for  grove  picking.  This  robot  was  hydraulically  servo-actuated  and 
equipped  with  position  and  velocity  sensors  on  each  joint.  A  computer  was  interfaced  to  the 
robot  and  a  conventional  high-level  computer  programming  language  was  used  to  program 
picking  intelligence.  A  flow  chart  technique  was  used  to  represent  the  picking  logic.  As  new 
logic  was  incorporated  into  the  program  to  solve  problems  associated  with  grove  operation,  the 
flow  chart  was  updated  to  reflect  these  changes.  Maintaining  the  complex  flow  chart  soon 
became  unmanageable  as  additional  problems  associated  with  grove  operation  were  recognized 
and  solved.  It  became  apparent  that  a  different  method  of  specifying  intelligence  was 
necessary  before  additional  grove  related  problems  could  be  solved.  It  was  the  goal  of  this 


research  to  develop  a  programming  technique  and  to  develop  the  intelligence  required  to 
remove  fruit  grown  in  production  conditions. 


CHAPTER  2 
RESEARCH  OBJECTIVES 

The  overall  objective  of  this  research  was  to  develop  the  machine  intelligence  required 
to  pick  fruit  using  a  vision-servoed  robot.  The  specific  objectives  were  to 

1 .  define  a  robot  programming  language  for  specifying  fruit-picking  intelligence; 

2.  develop  the  intelligence  required  to  pick  oranges  grown  in  production  conditions;  and 

3.  evaluate  the  adequacy  of  the  language  and  picking  intelligence. 

The  programming  language  was  to  provide  a  consistent  framework  for  development  of  a 
picking  program.  Several  desired  features  of  a  robot  programming  language  included 
declaration  of  data  structures,  decision  making  capabilities,  and  input  and  output  capabilities. 
The  programming  language  was  to  be  modular  in  nature  to  facilitate  making  modifications  to 
the  existing  program.  Debugging  tools  for  the  picking  program  would  be  developed  to  aid  in  the 
refinement  of  the  picking  intelligence.  These  debugging  tools  were  to  include  routines  for 
recording  robot  information  in  real  time  and  displaying  robot  information  on-line. 

The  logic  to  execute  a  vision-servoed  picking  cycle  and  to  solve  many  of  the  problems 
associated  with  grove  operation  was  to  be  developed.  A  basic  picking  cycle  was  to  provide  the 
logic  to  search,  locate,  and  remove  fruit  from  a  tree.  The  basic  picking  cycle  was  to  be  utilized 
to  successfully  pick  fruit  that  were  unobstructed,  relatively  motionless,  and  within  reach  of  the 
robot.  Since  these  conditions  were  not  characteristic  of  all  fruit  encountered  during  actual  grove 
operation,  the  intelligence  was  to  be  extended  to  solve  the  following  problems: 

1 .  attempting  to  pick  fruit  that  were  outside  the  robot  work  space; 

2.  excessive  fruit  motion  that  prevented  a  successful  picking  attempt; 

3.  fruit  that  vanished  from  view  during  a  pick  cycle; 
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4.  picking  fruit  that  were  partially  occluded  by  limbs  and  leaves;  and 

5.  collisions  between  the  robot  and  a  tree. 

The  adequacy  of  the  robot  programming  language  was  to  be  evaluated.  The  evaluation 
was  to  consider  ease  of  specifying,  modifying,  and  debugging  the  picking  intelligence.  The 
adequacy  of  the  picking  intelligence  was  to  be  evaluated  by  documenting  the  ability  of 
successfully  removing  fruit  under  the  conditions  listed  above.  Creation  of  an  editor  and 
compiler  for  the  programming  language  were  not  attempted  in  this  work. 


CHAPTER  3 
LITERATURE  REVIEW 


This  chapter  reviews  topics  related  to  control  of  robotic  manipulators  and  robotic  fruit 
picking.  These  include  robots  developed  by  various  researchers  to  pick  fruit,  other  intelligent 
robotic  systems,  and  programming  techniques.  A  review  of  fruit-picking  robots  provides 
background  information  on  previous  work  done  in  this  field.  Intelligent  robots  developed  for 
applications  other  than  fruit  picking  are  reviewed  to  show  how  control  is  divided  between 
different  subsystems.  A  brief  review  of  programming  techniques  is  included  to  evaluate 
different  methods  of  programming  a  robot.  Levels  and  requirements  of  programming  languages 
are  reviewed  to  form  a  basis  for  language  design  and  evaluation. 

Fruit-Picking  Robots 

The  purpose  of  this  section  is  to  review  concepts  of  fruit  picking  and  fruit-picking 
robots.  Mechanization  of  citrus  harvest  was  discussed  by  Schertz  and  Brown  (1968).  Although 
the  word  robot  was  not  used,  this  paper  established  many  concepts  of  robotic  fruit  picking. 
Various  components  of  the  harvesting  functions — selection,  detachment,  collection,  and 
transportation — were  discussed  in  relation  to  individual  fruit  and  mass  harvesting  approaches. 
The  main  concerns  with  mass  harvesting  techniques  were  tree  damage  and  injury  as  fruit 
dropped  through  the  tree  to  a  collection  device.  Detachment  devices  and  methods  to 
implement  picking  operations  were  discussed  for  individual  fruit  harvesting.  It  was  concluded 
that  a  blind  penetration  of  a  detachment  device  at  prescribed  grid  points  on  the  canopy  would 
result  in  a  low  successful  pick  per  attempt  ratio.  A  visual  method  that  would  make  use  of  color 
properties  of  a  scene  to  discriminate  between  fruit  and  leaves  was  proposed  to  improve 
efficiency.  In  tests  to  determine  interference  of  foliage  on  optically  locating  fruit,  it  was 
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determined  that  70  to  100%  of  fruit  on  a  tree  were  observable  through  a  tube.  This  line-of-sight 
concept  implied  that  an  optical  sensor  located  in  a  tube  would  view  a  majority  of  fruit  on  a  tree. 

The  feasibility  of  a  fruit  (apple)  harvesting  system  using  visual  scene  analysis  was 
demonstrated  by  Parrish  and  Goksel  (1977).  A  black  and  white  vision  system  was  used  to  obtain 
scene  images.  An  image  was  processed  and  the  image  coordinates  of  apple  centroids  were 
determined.  A  trajectory  was  then  calculated  for  each  fruit  located  by  the  vision  system.  A 
three  degree-of-freedom  robotic  manipulator  with  a  prismatic  terminating  joint  was  moved 
along  a  trajectory  in  a  dead  reckoning  manner  until  fruit  contact  was  sensed  by  a  touch  sensor. 
Fruit  were  manually  removed  since  a  detachment  mechanism  had  not  been  developed.  The 
trajectories  to  the  remaining  fruit  were  then  serially  executed.  The  vision  system  examined  the 
scene  again  to  detect  the  presence  of  additional  fruit.  If  additional  fruit  could  not  be  located, 
the  vision  system  was  panned  and  tilted  to  view  a  different  portion  of  the  scene. 

A  robotic  apple  harvester  was  described  by  Grand  d'Esnon  (1985).  This  system  consisted 
of  a  hollow  tube  with  a  picking  device  attached  at  the  end,  a  charged  coupled  device  (CCD) 
line  scan  camera,  and  a  computer  control  system.  The  arm  and  camera  were  mounted  in  a  fixture 
carriage  that  could  be  vertically  moved.  As  the  carriage  was  moved  upward,  horizontal  video 
line  scans  were  acquired.  When  a  fruit  was  detected,  the  arm  was  aligned  horizontally  and 
moved  outward  toward  the  fruit  in  a  dead  reckoning  manner.  A  sensor  detected  proximity  and  a 
picking  device  removed  the  fruit.  The  fruit  rolled  down  the  hollow  tube  into  a  collection 
device.  A  laboratory  cycle  rate  of  one  fruit  every  four  seconds  was  realized. 

Based  on  experiments  with  this  robot,  a  new  prototype  was  designed  and  constructed  by 
Grand  d'Esnon  et  al.  (1987).  This  harvester  was  a  self-propelled,  self-guided  vehicle  that 
included  a  hydraulic  manipulator  and  a  collection  system.  The  manipulator  used  a  vacuum 
picking  head  and  was  mounted  on  an  elevator  arm  that  contained  a  fruit  conveyor  system. 
Three  cameras  for  color  imaging  were  mounted  at  the  base  of  the  manipulator  to  provide  scene 
information.  After  a  fruit  was  located  by  the  vision  system,  the  forearm  was  moved  to  align 
with  the  visually  estimated  horizontal  and  vertical  locations  of  the  fruit.  The  elbow  joint 
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then  moved  to  provide  a  straight-line,  dead  reckoning  approach  to  the  fruit.  Fruit  contact  was 
detected  by  monitoring  the  pressure  on  the  vacuum  system  in  the  picking  device.  After  contact 
was  made,  the  arm  retracted  and  moved  to  the  conveyor  belt  to  release  the  fruit.  Under 
acceptable  lighting  conditions,  the  robot  picked  more  than  50%  of  all  fruit  from  a  tree  at  a  cycle 
rate  of  one  fruit  every  four  seconds. 

Tutle  (1984)  proposed  a  robotic  system  for  orange  harvesting  that  consisted  of  three 
harvester  modules.  Each  module  would  contain  robotic,  imaging,  and  collection  systems.  A 
global  imaging  system  would  be  used  to  obtain  scenes  for  orange  location  in  different  zones.  The 
imaging  system  would  be  positioned  at  the  pivot  center  of  the  robotic  system.  After  scene 
analysis,  the  imaging  sensor  would  be  moved.  The  robotic  arm  would  then  be  moved  along  a 
calculated  trajectory  to  a  fruit.  A  system  in  the  picking  device  would  determine  proximity  to  an 
object  by  detecting  light  energy  reflected  from  the  object.  Final  alignment  of  the  robot  arm 
would  be  made  using  this  sensor.  The  system  was  intended  to  harvest  fruit  at  night  to  avoid 
imaging  problems  associated  with  daytime  operation. 

An  experimental  robot  was  developed  by  Kawamura  et  al.  (1983a)  to  harvest  tomatoes. 
The  self-propelled  robot  consisted  of  a  color  camera,  a  microcomputer,  and  a  manipulator 
mounted  on  a  mobile  battery-operated  car  to  travel  between  crop  rows.  The  manipulator 
constructed  for  the  robot  was  discussed  in  Kawamura  et  al.  (1983b).  Limit  switches  were  used  to 
detect  row  edges  for  vehicle  guidance  between  tomato  rows.  Different  images  of  the  tomato 
scene  were  used  to  determine  the  three  dimensions  of  a  tomato.  The  position  information  was 
used  to  calculate  a  dead  reckoning  harvesting  trajectory  for  the  manipulator.  A  removal  rate  of 
one  tomato  every  15  to  20  seconds  was  achieved. 

Fruit  movement  is  a  problem  associated  with  using  the  dead  reckoning  technique  during 
fruit  approach.  With  dead  reckoning,  once  a  trajectory  has  been  calculated  fruit  position  is  not 
updated  while  the  picking  device  is  moved  toward  a  fruit.  If  fruit  movement  occurs  after  the 
initial  targeting,  the  picking  attempt  could  fail  due  to  misalignment  of  a  picking  device  with  a 
fruit.  Systems  that  used  analysis  of  one  image  scene  to  target  several  fruit  would  also  encounter 


problems  after  a  fruit  was  removed.  The  calculated  trajectories  to  other  fruit  may  be 
invalidated  because  the  weight  of  removed  fruit  could  affect  remaining  fruit  locations. 

To  avoid  the  dead  reckoning  problem,  Harrell  et  al.  (1985)  interfaced  a  commercial 
robot  and  controller  to  a  real-time  machine  vision  system.  A  black  and  white  camera  was 
mounted  inside  a  picking  device  at  the  end  of  a  prismatic  joint.  Images  were  processed  in  real- 
time to  determine  the  horizontal  and  vertical  positions  of  a  targeted  fruit.  Horizontal  and 
vertical  image  offsets  were  used  to  control  horizontal  and  vertical  revolute  joints  in  real  time. 
The  prismatic  joint  was  extended  toward  a  fruit  while  the  two  revolute  joints  tracked  an  object 
located  by  the  vision  system.  The  robot  could  track  fruit  as  long  as  fruit  movement  was  not  too 
rapid.  Object  diameters  were  used  to  indicate  when  the  picking  device  was  close  to  a  fruit.  A 
curved  piece  of  metal  served  as  the  picking  device  and  rotated  behind  the  fruit.  The  stem  was 
impinged  between  picking  lip  and  the  top  of  the  end-effector.  Withdrawal  of  the  picking 
device  from  the  canopy  severed  the  stem.  Single  pick  cycle  times  of  3  to  7  seconds  were  obtained 
during  laboratory  performance. 

Moving  the  technology  to  the  grove  setting  (Harrell  et  al.,  1988)  required  development 
of  a  color  imaging  system  (Slaughter,  1987)  to  differentiate  citrus  from  background  leaves,  soil, 
and  sky.  A  Bayesian  statistical  method  was  used  to  classify  color  combinations  into  foreground 
(orange)  or  background  classes.  To  obtain  data  for  the  statistics,  image  scenes  were  acquired  and 
a  foreground/background  decision  was  made  of  selected  pixels  in  the  image.  This  data  set  were 
used  to  define  the  different  color  combinations  that  constituted  each  color  class. 

Intelligent  Robots 

Robots  have  generally  been  used  to  perform  repetitious  tasks  in  a  highly  structured 
work  environment.  To  extend  the  application  of  robots  to  unstructured  environments,  sensing 
environmental  conditions  and  responding  to  events  are  necessary.  This  has  resulted  in  more 
complex  robot  systems.  Chun  (1987)  divided  the  components  of  a  robot  system  into  power  source, 
locomotion,  control,  sensor  system,  and  manipulator  subsystems.  The  power  source  subsystem 
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supplies  energy  for  all  other  subsystems.  The  design  of  the  locomotion  subsystem  is  highly 
dependent  on  the  environment  in  which  the  robot  will  operate.  The  control  subsystem  supplies 
command  signals  for  navigation  and  the  sensor  subsystems.  The  sensor  subsystems  contain  any 
device  used  in  obtaining  environmental  information.  The  manipulator  subsystem  is  dependent 
on  the  environment  in  which  the  robot  will  operate  and  the  task  that  the  robot  must  perform. 

One  remotely  controlled,  6-legged  mobile  robot  that  has  been  developed  and  refined  is 
ODEX  (Bartholet,  1987).  The  control  system  was  hierarchical  in  nature  and  included  a  low- 
level  processor  dedicated  to  each  leg  and  a  processor  for  managing  distribution  and  data  flow 
between  leg  processors  and  a  high-level  system.  The  high-level  system  contained  several 
processors  to  perform  the  walking  algorithms,  vehicle  system  diagnostics,  communications,  and 
auxiliary  support  functions. 

Discussion  of  another  intelligent  robot  can  be  found  in  Andersson  (1988).  This  robot 
played  ping-pong  and  required  environmental  sensing,  trajectory  planning,  and  execution  of 
motions  in  real  time.  The  control  system  was  partitioned  into  a  network  of  microprocessors.  A 
vision  system  was  used  to  locate  the  ping-pong  ball  in  three  dimensions  and  send  data  to  a 
trajectory  analyzer.  The  trajectory  analyzer  was  used  to  predict  motion  of  the  ball  and  send 
data  to  a  strategic  analyzer  to  analyze  the  opponent's  strategy.  The  trajectory  analyzer  also 
sent  predicted  ball  information  to  the  real-time  expert  controller.  The  expert  controller 
planned  robot  motion  and  commanded  servo  processors  to  move  robot  joints.  A  user  interface 
provided  user  control  and  debugging  facilities.  This  robot  system  required  dedicated  systems  to 
perform  separate  activities  because  of  time  constraints  imposed  by  real-time  control. 

A  sheep-shearing  robot  was  described  by  Key  (1985).  This  robot  consisted  of  a 
manipulator  for  restraining  and  positioning  a  sheep  and  a  robotic  arm  used  for  shearing.  The 
shape  of  each  sheep  was  different  and  changed  due  to  animal  movement  and  breathing.  Prior 
to  shearing,  a  surface  model  was  predicted  by  using  several  measurements  of  the  sheep  and  a 
data  base  of  sheep  measurements.  The  robot  cutting  arm  utilized  programmed  shearing 
movements  according  to  this  contour  map.  Adaption  of  the  surface  model  had  to  be  performed 
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during  the  shearing  process  to  compensate  for  sheep  movement  and  surface  variations. 
Feedback  from  the  cutter  head  was  also  required  for  adaption  of  the  cutting  depth  because  of 
sheep  movement. 

A  common  factor  with  these  intelligent  robots  is  a  separation  of  intelligence  components 
from  low-level  joint  control.  Separate  computer  processors  are  dedicated  to  handle  selected 
problems  involved  in  performing  a  robot  task.  This  separation  of  duties  provides  a  modular 
approach  to  solving  problems. 

Programming  Techniques 

Programming  robotic  manipulators  has  been  the  subject  of  research  for  over  thirty  years. 
As  manipulators  and  sensors  increase  in  sophistication,  the  need  for  flexibility  in  programming 
has  grown.  More  information  from  the  mechanism  and  environment  requires  more  computer 
processing  time  and  intelligent  decision  making.  Tasks  that  are  increasingly  complex  and  the 
need  for  more  robot  and  task  intelligence  has  resulted  in  an  array  of  programming  techniques. 
Programming  robotic  manipulators  has  progressed  from  simple  teach-box  point-to-point  control 
to  higher  level  programming  languages  that  incorporate  human-like  reasoning.  By  including 
more  environmental  information  in  decision  making,  a  manipulator  can  perform  more  complex 
and  interactive  tasks. 

Different  programming  techniques  exist  to  generate  robot  programs.  These  include 
teach  programming  and  off-line  textual  programming  (Blaha  1986).  Teach  programming  is  the 
oldest  method  used  to  generate  robot  programs.  Methods  of  teach  programming  vary  from 
manually  moving  the  robot  end-effector  through  desired  motions  to  using  a  teach  pendant  that 
has  switches  and  push  buttons  for  commanding  joint  movement,  end-effector  actions,  and  other 
features.  Advantages  of  teach  programming  are  ease  of  learning  and  the  availability  on 
industrial  robots.  Instead  of  a  computer  programmer,  an  operator  who  is  familiar  with  the 
application  can  develop  a  program,  since  the  programming  method  is  easily  learned.  However, 
there  are  disadvantages  to  this  programming  method.  Since  the  robot  is  used  during  a 


programming  session,  the  robot  cannot  be  performing  other  production  tasks.  Tasks  that  require 
coordination  with  other  machinery  in  the  work  cell  require  them  to  be  dedicated  to  the 
programming  session. 

Due  to  the  disadvantages  of  teach  programming,  several  off-line  textual  programming 
languages  were  developed.  Developers  of  these  languages  took  different  approaches:  either 
basing  the  language  on  numerical  control  languages;  extending  high-level  computer  languages; 
or  developing  a  new  language  from  the  ground  up.  Some  advantages  of  off-line  textual 
programming  are  1)  the  robot  can  be  performing  a  task  while  a  new  program  is  developed,  2) 
additional  sensor  data  can  be  incorporated  more  easily  than  with  teach  programming,  3) 
branching  and  looping  constructs  enhance  error  handling,  and  4)  repetitive  subtasks  can  be 
programmed  and  stored  as  macros  or  subroutines.  There  are  several  disadvantages  of  off-line 
textual  programming.  The  language  syntax  must  be  learned  before  a  robot  program  can  be 
developed.  Another  disadvantage  is  the  difficulty  in  visualizing  an  end-effector  path  in  three 
dimensions.  Orientation,  collision-free  paths,  reachability,  and  object  locations  are  hard  to 
visualize  when  using  a  textual  programming  method. 

Blaha  et  al.  (1986)  classified  the  levels  of  robot  programming  languages  as  servo, 
manipulator,  and  task.  The  lowest  level  of  programming  (servo)  uses  a  series  of  end  points,  joint 
speeds,  and  input  and  output  commands  to  control  a  robot.  At  the  manipulator  level,  the 
language  has  explicit  motion  commands,  some  sensor  capability,  and  branching  and  looping 
constructs.  A  manipulator  level  language  would  have  explicit  commands  for  an  end-effector.  To 
move  an  object  from  one  location  to  another,  the  programmer  would  have  to  specify  individual 
steps  to  accomplish  the  task.  For  example,  the  program  might  include  the  following 
statements:  open  end-effector,  move  to  location_one,  close  end-effector,  move  to  location  two, 
and  open  end-effector.  At  the  task  level,  the  language  provides  for  task  specification.  An 
example  of  a  task  level  program  is:  put  block  A  on  block  B.  The  language  compiler/interpreter 
has  to  break  down  the  programmed  statement  into  a  sequence  of  robot  actions  to  accomplish  the 
task.  Different  levels  of  sophistication  exist  within  each  programming  level.  For  example,  at 
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the  high  end  of  task  level  programming,  the  robot  programmer  might  specify  build  engine.  The 
compiler/interpreter  must  be  able  to  break  down  the  statement  into  a  series  of  robot  commands 
to  accomplish  the  task.  Clearly,  the  more  sophisticated  programming  languages  have  a  more 
sophisticated  compiler/ interpreter. 

Bonner  and  Shin  (1982)  grouped  different  programming  languages  into  five  levels  of 
sophistication — hardware,  point-to-point,  motion,  structured  programming,  and  task-oriented. 
At  the  hardware  level  of  programming,  the  programmer  specifies  actual  actuator  commands. 
The  point-to-point  level  adds  the  ability  to  store  a  list  of  desired  joint  positions  and  the 
capacity  to  sequentially  move  the  robot  joints  through  the  series  of  joint  positions.  Motion 
control  is  used  to  specify  how  a  robot  joint  will  move  between  two  positions.  Instead  of 
specifying  the  position  of  each  joint  in  joint  compatible  units,  coordinate  frames  are  often 
established  for  each  robot  joint.  The  programmer  can  specify  robot  poses  in  Cartesian 
coordinates  and  specify  absolute,  relative,  or  straight-line  motion  between  joint  positions.  A 
motion  level  programming  language  adds  another  level  of  sophistication  around  joint 
controllers  with  the  use  of  additional  software  to  calculate  joint  trajectories.  Advantages  of 
motion  control  are  the  addition  of  flow  control  (task  branching  and  subroutines)  and  an 
increased  capacity  to  use  sensor  information. 

The  structured  and  task-oriented  levels  of  programming  add  more  sophistication  to 
programming  a  robot  task.  Structural  programming  languages  generally  provide  more  complex 
data  structures,  data  storage,  and  extensive  use  of  coordinate  transformation  routines.  A  task- 
oriented  programming  language  conceals  robot-related  information  and  includes  complex 
modeling  systems,  human-like  decision-making  capacity,  and  interactive  debugging  systems. 

As  the  level  of  programming  increases  from  the  hardware  level  to  the  task-oriented 
level,  there  is  a  dependency  on  previous  levels  of  programming.  The  point-to-point 
programming  level  uses  the  hardware  and  software  of  the  hardware  programming  level.  At 
the  sophisticated  task-oriented  level,  the  programmer  may  specify  the  complete  robot  task  in 
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one  command.  As  the  level  of  programming  sophistication  increases,  the  language  is  further 
removed  from  robot-specific  hardware. 

Programming  a  task  for  commercial  robots  has  generally  made  use  of  point-to-point 
programming  methods.  A  task  is  performed  by  sequential  stepping  through  a  series  of 
memorized  position  set  points.  This  type  of  programming  works  well  for  robot  tasks  performed 
in  a  highly  structured  work  environment.  Programming  a  robot  to  work  in  the  unstructured 
agricultural  environment  makes  intensive  use  of  environmental  sensor  data.  Obviously, 
programming  a  robot  to  pick  fruit  cannot  be  made  using  a  teach  programming  method  since  fruit 
positions  are  not  known  a  priori.  Adaption  of  an  existing  robot  programming  language  was  not 
suited  to  the  task  of  picking  fruit  for  many  reasons.  Adding  new  sensors  to  quantify  additional 
environmental  conditions  and  inclusion  of  additional  sensor  information  in  decision-making 
capabilities  are  limited.  Since  fruit  position  would  not  be  known  a  priori,  a  vision  system 
would  be  utilized  to  detect  two  dimensions  of  fruit  position.  Using  vision  feedback  for  joint 
control  required  feedback  controllers  to  be  designed  and  implemented  with  joint  controller 
hardware.  The  intelligence  to  pick  fruit  would  require  the  ability  to  specify  different  modes  of 
joint  feedback. 

Requirements  of  a  robot  programming  language  were  reported  by  Lozano-Perez  (1983)  as 
sensing,  world  modeling,  motion  specification,  flow  of  control,  and  programming  support.  Use  of 
environmental  information  such  as  force,  vision,  torque,  touch,  and  proximity  can  be  used  to 
increase  the  intelligent  operation  of  the  robot.  The  capacity  to  utilize  sensor  information  to 
recognize  and  adapt  to  environmental  changes  is  essential  if  a  robot  must  work  in  an 
unstructured  environment.  The  sensing  requirement  is  reflected  in  world  modeling  capabilities 
and  flow  control.  When  the  environment  is  not  known  a  priori,  modeling  techniques  convert 
sensor  input  into  information  that  can  be  used  during  task  execution.  An  example  of  modeling  is 
converting  information  from  a  vision  system  into  world  coordinates  so  a  robot  can  manipulate 
objects.  Use  of  sensor  information  to  alter  program  flow  is  essential  to  adapt  to  changes  in  the 
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environment.  Programming  support  for  the  language  is  required  to  generate,  debug,  and  simulate 
the  robot  program. 

Evaluation  of  a  programming  language  involves  examining  language  features  and 
capabilities.  The  ease  that  the  language  can  be  used,  transported,  maintained,  and  expanded 
are  language  capabilities  that  determine  the  overall  appropriateness  of  a  language  to  a 
particular  robot  task.  Language  features  include  elements  that  are  common  to  conventional 
computer  languages  (declaring  variables,  data  types,  data  operators,  control  structures, 
subprograms,  input,  and  output)  and  elements  that  are  specific  to  robot  programming  languages 
(motion  control  and  tool  statements). 

Language  capabilities  include  flexibility,  usability,  extensibility,  portability, 
reliability,  maintainability,  decision  making,  and  sensor  support  (Blaha  1988).  The  range  of 
applications  in  which  a  programming  language  can  be  used  is  a  measure  of  the  flexibility  of  the 
language.  How  well  program  development  meets  cost  and  maintainability  guidelines  is  a 
measure  of  the  usability  of  a  language.  Extensibility  represents  the  ability  to  create  abstract 
data  structures  to  represent  a  problem.  Portability  represents  how  easily  a  programming 
language  can  be  ported  to  other  systems  and  is  a  function  of  how  removed  a  language  is  from 
controller  hardware.  Reliability  is  tied  to  program  development  and  implementation  and 
deals  with  the  ability  to  detect  syntactic  and  semantic  errors  prior  to  execution  of  a  program. 
Maintainability  is  a  measure  of  how  easily  a  robot  program  can  be  written,  debugged, 
documented,  and  modified.  Decision  making  is  a  measure  of  how  well  a  language  can 
implement  both  implicit  and  explicit  branching  to  other  parts  of  a  robot  program.  Sensor 
support  is  a  measure  of  how  well  a  language  supports  information  from  a  wide  range  of  sensors. 


CHAPTER  4 
THE  VISION-SERVOED  ROBOT 


The  purpose  of  this  chapter  is  to  provide  a  background  of  the  hardware  and  basic  input 
and  output  software  components  comprising  the  fruit-picking.  The  chapter  is  divided  into  a 
description  of  the  fruit-picking  robot,  control  system  hardware,  control  system  software,  and 
debugging  facilities.  A  general  overview  of  the  robot  support  system  will  be  presented  first. 
Robot  construction  details  will  then  be  presented.  The  control  system  hardware  and  software 
will  be  discussed  to  provide  background  information  for  software  development. 

A  field  laboratory  was  constructed  to  provide  researchers  with  portable  facilities  to 
develop  and  evaluate  vision-servoed  fruit-sensing  technologies.  The  field  laboratory  was 
constructed  on  a  2  m  by  6  m,  dual  axle  trailer  divided  into  an  enclosed  control  room  and  an  open 
rear  deck.  The  layout  of  the  control  room  and  the  rear  deck  are  shown  in  Figure  4.1.  The  control 
room  occupied  the  front  portion  of  the  trailer  bed  and  contained  two  work  stations,  a  power 
conditioning  unit,  and  a  robot  control  computer.  An  electrical  power  generator,  a  hydraulic 
power  unit  (HPU),  and  a  fruit-picking  robot  were  located  on  the  open  rear  deck.  A  15  KW 
generator  actuated  by  an  air-cooled  gasoline  engine  provided  3<t>,  208  VAC  power.  Trailer 
power  could  be  supplied  from  the  generator  for  remote  operation  or  an  external  power  source  for 
stationary  laboratory  operation. 

Fruit-Picking  Robot 

The  three  degree-of-freedom  robot  used  to  study  fruit  picking  is  shown  in  Figure  4.2. 
The  robot  contained  three  hydraulically  actuated  servo-joints  and  an  end-effector.  The  end- 
effector  included  a  fruit-picking  device  and  a  sensor  package  for  end-of-the-arm  fruit  sensing. 
The  robot  arm  was  constructed  from  a  hollow  aluminum  cylinder  and  enclosed  in  a  Hooke  joint 

for  rotational  movement  in  the  horizontal  and  vertical  planes.  A  servo-drive  attached  to  joint 
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Components: 

12.  Markboard 

1 .  Computer  cabinet  (below  table) 

13.  Door 

2.  Electrical  connector  rack 

14.  Electrical  generator  (main  unit) 

3.  Printer 

15.  Electrical  generator  (meter  box) 

4.  Interior  breaker  box 

16.  Hydraulic  power  unit  (HPU) 

5.  Tool/storage  box 

17.  Computer  connector  box 

6.  Book  shelf 

18.  Hydraulic  fluid  cooler 

7.  Joy  sticks  (2) 

19.  Manipulator 

8.  Plotting/  printing  console 

20.  Elect  leal  power  box 

9.  RGB  monitor 

21.  Gas  tank  fill  cap 

10.  Main  system  console 

22.  Windows 

1 1 .  Control  room  table 

23.  Power  conditioner  ftjelow  table) 

22  20 
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Figure  4.1  Layout  of  trailer  components. 
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Figure  4.2  The  fruit-picking  robot 
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0  rotated  the  robot  arm  in  a  vertical  plane  about  axis  0.  Joint  motion  about  axis  0  was 
mechanically  limited  to  approximately  ±40  degrees.  A  servo-drive  attached  to  joint  1  rotated 
the  robot  arm  in  a  horizontal  plane  about  axis  1.  Joint  motion  about  axis  1  was  mechanically 
limited  to  approximately  ±23  degrees.  The  robot  arm  was  moved  in  and  out  along  axis  2  by  a 
servo-drive  attached  to  joint  2.  Joint  motion  along  axis  2  was  mechanically  limited  to 
approximately  1.3  m.  A  complete  description  of  robot  construction  can  be  found  in  Pool  (1989). 

A  pressure-compensated  hydraulic  pump  actuated  by  an  electric  motor  generated 
hydraulic  fluid  pressure  for  the  robot.  The  hydraulic  circuit  for  the  HPU  is  shown  in  Figure  4.3. 
Remote  control  of  hydraulic  fluid  direction  was  provided  by  an  electric  solenoid  valve. 
Deactivation  of  the  solenoid  valve  (called  the  HPU  dump  valve)  directed  hydraulic  fluid 
back  to  the  HPU  reservoir.  Activation  of  the  HPU  dump  valve  directed  hydraulic  fluid  to  the 
robot.  A  ball  valve  provided  manual  control  of  hydraulic  fluid  to  the  manipulator.  Thus, 
hydraulic  power  to  the  robot  could  be  removed  by  either  deactivating  the  HPU  dump  valve  or 
closing  the  ball  valve.  A  one  liter  accumulator  was  utilized  to  store  hydraulic  energy  for  use 
when  robot  actuators  demanded  a  higher  flow  rate  than  the  hydraulic  pump  could  supply. 
Hydraulic  fluid  was  distributed  to  actuators  from  the  pressure  manifold.  Return  of  hydraulic 
fluid  from  the  actuators  joined  at  the  return  manifold  and  passed  through  a  forced-air  fluid 
cooler  before  returning  to  the  reservoir. 

The  hydraulic  circuit  of  joints  0  and  1  is  shown  in  Figure  4.4  A  and  the  hydraulic  circuit 
used  for  joint  2  is  shown  in  Figure  4.4  B.  Servo-valves  were  used  to  supply  fluid  to  actuators  for 
joint  motion.  Control  signals  to  the  servo-valves  were  generated  by  servo-amplifiers.  Rotary 
actuators  were  used  on  joints  0  and  1.  A  hydraulic  motor  was  used  on  joint  2  which  drove  a  pinion 
gear  meshed  with  a  rack  mounted  on  the  robot  arm. 

Output  from  precision  potentiometers  and  tachometers  attached  to  the  robot  joints  were 
used  to  measure  joint  positions  and  velocities.  A  DC  voltage  was  supplied  across  each 
potentiometer's  leads.  The  voltage  drop  across  one  potentiometer  lead  and  a  pick  off  lead  was 
utilized  to  indicate  joint  position.  Tachometers  attached  to  joints  0  and  1  generated  a  voltage 
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proportional  to  joint  angular  velocity.  A  tachometer  attached  to  joint  2  generated  a  voltage 
proportional  to  the  linear  velocity  of  joint  2. 
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Figure  4.3  Hydraulic  circuit  for  hydraulic  power  unit. 
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Figure  4.4  Hydraulic  circuits  for  joint  actuators.  A)  Hydraulic  circuit  for  joints  0  and  1. 
B)  Hydraulic  circuit  for  joint  2  actuator. 
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A  picking  device  was  mounted  at  the  end  of  joint  2  (Figure  4.5)  and  consisted  of  a  curved 
piece  of  metal  horizontally  attached  to  the  sides  of  the  end-effector.  The  device  rotated 
behind  fruit  and  impinged  the  stem  between  the  lip  and  a  collection  scoop.  Withdrawal  of  the 
robot  arm  from  the  canopy  severed  the  stem.  The  picking  device  was  activated  by  a  double 
acting  hydraulic  cylinder  mounted  at  the  rear  of  the  robot  arm.  A  linkage  bar  connected  the 
hydraulic  cylinder  rod  to  a  lever  arm-sprocket  assembly  located  in  the  end-effector.  A  three- 
way  solenoid  valve  directed  hydraulic  fluid  to  the  picking  device  hydraulic  cylinder.  The 
hydraulic  circuit  used  for  the  picking  device  is  shown  in  Figure  4.6.  Activation  of  one  solenoid 
supplied  fluid  to  one  end  of  the  cylinder  and  caused  the  picking  device  to  rotate  to  the  up  or 
closed  position  used  to  catch  fruit.  Activation  of  the  other  solenoid  rotated  the  picking  device 
to  the  down  or  open  position  (shown  in  Figure  4.5)  and  was  used  to  release  fruit.  The  position  of 
the  picking  device  was  not  sensed. 


Figure  4.5  Picking  device  mechanical  linkage. 

A  sensor  package  located  in  the  end-effector  consisted  of  a  color  camera,  an  ultrasonic 
transducer,  and  a  lighting  system  (Figure  4.7).  The  sensor  components  were  located  inside  the 
end-effector  to  provide  object  information  from  the  end  of  the  robot  arm.  The  camera  provided 
visual  information  of  the  environment.  The  ultrasonic  ranging  system  provided  distance 
information  to  an  object  located  in  front  of  the  end-effector.  Four  incandescent  lights  provided 
scene  illumination  when  the  end-effector  was  close  to  an  object. 
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Figure  4.6  Hydraulic  circuit  for  picking  device. 
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Figure  4.7  The  end-effector  sensor  package. 
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Control  System  Hardware 
The  control  system  hardware  consisted  of  a  control  computer;  two  user  consoles;  a  red, 
green,  and  blue  (RGB)  video  monitor;  and  two  joy  sticks  (Figure  4.8).  The  robot  control  computer 
consisted  of  a  mass  storage  device,  a  central  processing  unit  board,  and  several  boards  installed 
for  transmitting  and  receiving  information  to  and  from  the  robot.  The  user  consoles  and  the 
computer  were  linked  with  serial  ports.  A  terminal  (called  the  main  system  console)  was  used 
for  one  of  the  user  consoles  and  a  computer  system  (called  the  printer/plotter  console)  with  local 
data  storage,  mass  data  storage,  and  printing  capabilities  was  used  for  the  other  console.  A 
third  serial  port  was  used  for  remote  communications. 
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Figure  4.8  Robot  control  computer  components  and  connections. 


A  parallel  port  connected  to  an  input/output  panel  was  used  to  activate  120  VAC 
devices  and  sense  AC  and  DC  voltage  levels  to  other  components.  Four  solid  state  relays  were 
used  to  provide  120  VAC  power  to  the  HPU  dump  valve,  the  end-effector  lighting  system 
power  supply,  and  the  two  picking  device  solenoids.  A  schematic  of  the  HPU  dump  valve 
circuit  is  shown  in  Figure  4.9.  The  control  computer  switched  power  to  point  A  by  activating  a 
solid  state  relay.  Before  the  solenoid  valve  could  receive  power,  an  external  push  button  had  to 
be  closed.  The  status  of  power  to  the  solenoid  (beyond  the  external  push  button)  was  sensed  for 
safety  reasons.  Three  DC  sense  modules  were  used  to  monitor  the  status  of  the  push  button  on 
each  joy  stick  and  two  push  buttons  mounted  on  the  control  console.  Servo-amplifier  power  was 
also  sensed  for  safety  reasons. 

Analog  to  digital  (A/D)  converters  were  used  to  quantify  voltage  levels  from  sensor 
devices.  Joint  positions  were  obtained  by  digitizing  the  voltage  level  across  the  potentiometers 
attached  to  the  robot  joints.  Joint  velocities  were  obtained  by  digitizing  the  voltage  levels 
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Figure  4.9  Dump  valve  circuit  for  the  hydraulic  power  unit. 
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generated  by  tachometers  attached  to  the  robot  joints.  The  position  of  the  control  room  joy 
sticks  were  obtained  by  digitizing  the  voltage  levels  across  the  potentiometers.  Digital  to 
analog  (D/A)  converters  supplied  voltage  reference  signals  to  servo-amplifiers.  Three  servo- 
amplifiers  provided  a  current  signal  to  the  servo-valves  proportional  to  the  voltage  signal 
supplied  by  the  D/A  converter. 

The  signal  from  the  ultrasonic  sensor  system  was  a  TTL  pulse  with  the  high  state 
proportional  to  the  distance  between  an  object  and  the  ultrasonic  transducer.  The  TTL  pulse  was 
received  by  the  computer  at  a  40  Hz  rate.  A  counter  timer  board  quantified  the  duration  of  the 
ultrasonic  signal. 

Visual  information  from  the  environment  was  obtained  by  using  a  color  video  camera 
mounted  in  the  end-effector.  The  video  porting  diagram  was  given  in  Figure  4.8.  The  composite 
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signal  from  the  camera  was  divided  into  red,  green,  and  blue  signals  by  a  video  decoder  and  sent 
to  three  image  acquisition  boards.  Video  images  for  each  color  were  digitized  using  5  bit 
resolution  and  stored  in  a  384  horizontal  by  242  vertical  picture  element  (pixel)  field.  A  video 
field  was  acquired  at  a  60  Hz  rate  with  video  data  storage  alternating  between  two  interlaced 
fields.  Live  video  signals  and  RGB  digitized  images  could  be  displayed  on  the  RGB  monitor. 
The  red  storage  board  generated  a  60  Hz  interrupt  signal  coinciding  with  the  vertical  blanking 
signal  to  synchronize  control  software.  The  red  storage  board  also  generated  a  video  signal  for 
control  of  the  automatic  aperture  lens  mounted  on  the  camera. 

Control  System  Software 
This  section  presents  an  overview  of  the  driver  software  for  the  input  and  output 
facilities  of  the  robot.  Development  of  these  drivers  was  not  a  component  of  the  programming 
language  and  is  described  here  for  completeness.  An  in-depth  description  of  input  and  output 
driver  software  can  be  found  in  Pool  (1989).  Input  from  the  robot  included  joint  information 
(position  and  velocity)  and  information  from  the  sensor  package  located  in  the  end-effector 
(vision  and  ultrasonics).  Additional  information  was  obtained  from  control  room  joy  sticks,  the 
HPU  dump  valve,  the  servo-amplifier,  and  control  room  push  buttons.  Output  to  actuation 
devices  included  signals  to  robot  hydraulic  servo-valves,  the  aperture  opening  on  the  camera 
lens,  the  HPU  dump  valve,  the  picking  device,  and  the  end-effector  lighting  system.  Input  and 
output  driver  software  was  executed  at  a  real-time  rate  (60  Hz). 

Input  Drivers 

The  work  environment  was  sensed  using  input  drivers  to  quantify  signals  from  sensors. 
The  position  and  velocity  of  each  robot  joint  were  sensed  to  quantify  the  joint  location  and 
speed.  Joy  stick  potentiometer  offsets  were  sensed  to  allow  manual  control  of  joint  motion.  The 
ultrasonics  signal  provided  a  distance  measurement  from  the  end-effector  to  an  object.  Power 
sensing  inputs  established  the  status  of  power  to  five  devices.  Seven  vision  measurements 
provided  information  about  an  object  located  in  the  image. 


25 

Joint  position  (Figure  4.10)  was  obtained  by  digitizing  the  voltage  level  across  a 
potentiometer  attached  to  each  robot  joint  using  12  bit  A/D  converters.  The  dashed  box 
represents  actions  performed  by  the  joint  position  input  driver  software.  Information  entering  or 
enclosed  within  the  dashed  box  were  used  by  the  driver  software.  Information  exiting  the  box 
represented  data  calculated  by  the  driver.  The  data  type  (byte,  integer,  long  integer,  or  double) 
are  labeled  in  parenthesis.  An  A/D  offset  value  was  subtracted  from  the  A/D  value.  The 
difference  was  multiplied  by  a  conversion  gain  and  smoothed  using  a  digital  fourth-order  low- 
pass  filter.  The  A/D  offset  value  established  joint  zero  positions.  The  zero  positions  for  joints  0 
and  1  were  established  at  the  center  location  of  the  respective  joint  work  space.  The  zero 
position  for  joint  2  was  established  when  the  end-effector  was  fully  retracted.  The  gain 
converted  joints  0  and  1  locations  to  degrees  and  joint  2  location  to  centimeters.  The  sign  of  the 
gain  established  positive  and  negative  positions.  Positive  positions  for  joint  0  were  defined 
when  the  end-effector  was  above  the  horizon.  Positive  positions  for  joint  1  were  defined  when 
the  end-effector  was  rotated  away  from  the  control  room.  The  position  of  joint  2  was  always 
positive. 
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Figure  4.10  A  joint  position  input  driver. 

The  velocity  of  a  robot  joint  (Figure  4.11)  was  obtained  by  digitizing  the  voltage  level 
generated  by  a  joint  tachometer.  The  A/D  value  read  for  each  joint  velocity  was  multiplied  by 
a  gain  to  convert  the  value  to  degrees  per  second  for  joints  0  and  1  and  to  centimeters  per  second 
for  joint  2.  Rotation  of  the  tachometers  in  one  direction  generated  a  positive  voltage  potential 
while  rotation  in  the  other  direction  generated  a  negative  voltage  potential.  The  sign  of  the 
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gains  established  the  directions  for  positive  and  negative  velocities.  Positive  velocities  were 
defined  as  joint  movement  which  increased  joint  position  readings.  The  joint  velocities  were 
smoothed  using  a  digital  fourth-order  low-pass  filter. 


Figure  4.11  A  joint  velocity  input  driver. 
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The  position  of  the  joy  sticks  (Figure  4.12)  were  obtained  by  using  A/D  converter 
channels  to  digitize  the  voltage  drop  across  joy  stick  potentiometers.  An  A/D  offset  value  was 
subtracted  from  the  A/D  converter  reading  to  establish  a  joy  stick  offset  position.  The  joy  stick 
readings  were  not  filtered. 
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Figure  4.12  A  joy  stick  position  input  driver. 


joy  stick  offset  position  (integer) 


The  distance  from  the  end-effector  to  an  object  was  obtained  using  an  ultrasonic 
transducer  and  counter  timer  hardware.  The  distance  to  an  object  (Figure  4.13)  was  obtained  by 
counting  the  oscillations  of  a  reference  signal  while  the  ultrasonic  distance  signal  was  in  a  TTL 
high  state.  The  cycle  count  was  converted  to  an  object  distance  by  multiplication  of  a  gain.  A 
joint  2  position  offset  was  added  to  the  distance  to  determine  the  position  of  joint  2  needed  for 
fruit  removal.  The  effective  range  for  the  ultrasonic  transducer  was  approximately  9  to  40  cm. 
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Figure  4.13  The  distance  input  driver. 

The  power  status  of  the  HPU  dump  valve,  servo-amplifier,  console  push  buttons,  and 
joy  stick  push  buttons  were  sensed  for  safety  reasons.  The  power  input  driver  (Figure  4.14) 
sensed  power  by  using  voltage  sense  modules  installed  in  an  input/output  panel.  Data  from  the 
parallel  port  attached  to  the  input/output  panel  was  decoded  to  indicate  the  power  status  of 
the  devices.  An  output  of  1  represented  that  power  was  sensed  on  the  device  and  an  output  of  0 
represented  that  power  was  not  sensed  on  the  device. 
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Figure  4.14  The  power  input  driver. 


The  vision  input  driver  (Figure  4.15)  quantified  image  information  and  consisted  of 
color  segmentation,  object  location,  object  quantification,  and  average  pixel  intensity 
calculation  routines.  Color  segmentation  (Slaughter,  1987)  divided  each  color  combination  into 
one  of  two  classes— fruit  or  background.  A  look-up-table  created  off-line  contained  the  class 
information  for  each  color  combination.  Details  on  look-up-table  construction  can  be  found  in 
Harrell  et  al.  (1989).  Object  location  software  examined  selected  pixels  in  an  image  and 
utilized  the  look-up-table  to  determine  if  a  pixel  was  in  the  fruit  or  background  color  class.  If  a 
fruit-colored  pixel  was  located,  the  object  quantification  routine  was  executed.  Object 
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quantification  software  examined  pixels  in  the  vicinity  of  the  fruit-colored  pixel  located  by 
the  object  location  routine  to  determine  if  the  object  that  met  size  criteria  was  located  in  the 
image. 


horizontal  centroid  of  object  (long  integer) 
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Figure  4.15  The  vision  input  driver. 

An  illustration  of  the  object  location  routine  is  shown  in  Figure  4.16.  The  pixels  in  this 
figure  are  numbered  to  represent  the  order  of  class  evaluation  and  lines  with  arrows  indicate 
the  direction  of  the  search  pattern  (a  rectangular  spiral).  If  a  fruit  had  been  located  in  a 
previous  analysis,  the  search  pattern  started  at  the  horizontal  and  vertical  centroid  of  the 
fruit.  If  a  fruit  had  not  been  located  in  the  previous  analysis,  the  search  pattern  was  started  at 
the  center  of  the  image.  The  pixel  color  (from  the  look-up-table)  determined  if  the  search 
pattern  continued  or  the  object  quantification  routine  was  executed.  If  the  pixel  was  in  the 
background  class,  the  object  location  routine  continued  the  search  pattern. 

Since  pixel  0  classified  as  a  background  pixel,  a  horizontal  increment  was  used  to 
establish  the  next  image  location  to  examine  for  color  information.  After  the  class  evaluation 
of  pixel  1,  the  next  image  location  to  evaluate  was  determined  by  using  a  vertical  increment. 
During  the  search  pattern,  the  intensity  of  each  pixel  was  summed.  If  a  valid  object  was  not 
located,  the  average  intensity  of  background  pixels  was  calculated  for  use  in  aperture  control. 
The  object  quantification  routine  was  activated  when  pixel  51  was  reached  because  this  pixel 
classified  in  the  fruit-color  class. 
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Figure  4.16  Illustration  of  object  location  algorithm. 

The  object  quantification  routine  located  an  object's  horizontal  and  vertical  centroids 
and  determined  horizontal  and  vertical  diameters.  Figure  4.17  shows  the  method  utilized  to 
quantify  an  object.  The  pixel  located  by  the  object  location  routine  (pixel  0  in  Figure  4.17  A) 
provided  the  starting  image  location  for  object  quantification.  Every  fourth  pixel  horizontally 
to  the  right  of  pixel  0  was  examined  until  the  background  (pixel  1)  was  located  three  times. 
Location  of  the  background  three  times  prevented  an  early  abortion  of  the  boundary  search  due 
small  background  regions  within  an  object.  A  horizontal  chord  was  established  in  the  image  by 
examining  every  fourth  pixel  to  the  left  of  pixel  0  until  the  background  (pixel  2)  was  located. 
The  horizontal  chord  was  bisected  (pixel  3)  to  establish  the  starting  location  for  the  upper  and 
lower  boundary  search.  Location  of  the  lower  (pixel  4)  and  the  upper  (pixel  5)  boundaries 
established  a  vertical  chord.  Bisection  of  the  vertical  chord  (pixel  6)  established  a  new 
starting  location  for  the  next  horizontal  chord  determination. 

The  location  of  horizontal  and  vertical  chords  were  alternately  performed  three  times. 
The  second  chord  iteration  is  shown  in  Figure  4.17  B  and  the  final  horizontal  and  vertical 
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Notation  for  pixels: 

0  Starting  location  for  centroid  search 

1  Right  edge  of  object 
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4  Lower  edge  of  object 

5  Upper  edge  of  object 

6  Bisection  of  vertical  chord 


Figure  4.17  Illustration  of  object  quantification  algorithm.  A)  First  iteration  for  centroid 
location.  B)  Second  centroid  iteration.  C)  Final  centroid  iteration. 


chords  are  shown  in  Figure  4.17  C.  The  final  horizontal  and  vertical  chord  bisections  were  used 
as  the  centroid  locations  of  a  fruit.  Although  these  locations  were  not  always  the  exact 
centroid  locations,  the  locations  were  adequate  for  vision  servoing.  The  lengths  of  the  final 
horizontal  and  vertical  chords  were  used  as  the  horizontal  and  vertical  diameters  of  the  object. 
The  intensity  of  each  pixel  examined  during  the  last  horizontal  and  vertical  chord 
determination  was  summed  and  an  average  pixel  intensity  was  calculated  for  aperture  control  if 
the  object  classified  as  a  valid  object. 

An  object  was  classified  as  valid  if  both  the  horizontal  and  vertical  diameters 
exceeded  minimum  values.  The  diameters  and  centroids  of  a  valid  object  were  filtered  using  a 
digital  fourth-order  low-pass  filter.  If  a  valid  fruit  had  been  located,  the  filtered  diameters 
and  centroid  locations  were  saved.  The  next  image  analysis  would  start  at  this  object's  centroid 
locations.  If  the  object  did  not  classify  as  valid,  the  object  location  routine  was  resumed  until  a 
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valid  fruit  was  located  or  image  boundaries  were  encountered.  If  a  valid  fruit  could  not  be 
located  in  the  image,  the  value  of  the  horizontal  and  vertical  centroid  results  were  assigned 
the  value  of  image  center  locations  and  the  diameter  results  were  set  to  zero. 

Output  Drivers 

Five  output  drivers  were  used  to  calculate  and  transfer  data  to  actuation  devices  at  a  60 
Hz  rate.  Three  of  these  drivers  were  used  to  control  the  robot  hydraulic  actuators.  The  other 
drivers  were  used  to  control  the  camera  lens  aperture  opening  and  to  control  the  power  status  to 
the  HPU  dump  valve,  the  picking  device,  and  the  end-effector  lighting  system. 

The  output  drivers  developed  to  control  joint  movement  (Figure  4.18)  used  D/A 
converters,  servo-amplifier  channels,  servo-hydraulics,  and  feedback  calculations.  Different 
modes  of  closed-loop  feedback  control  were  used  for  joints  0, 1,  and  2.  Position  control  for  a  joint 
utilized  the  current  joint  position  (result  from  a  joint  input  driver),  a  position  set  point,  and  a 
lag-lead  control  law  algorithm  to  calculate  the  joint  servo-valve  control  word.  Velocity 
control  for  a  joint  utilized  the  current  joint  velocity,  a  velocity  set  point,  and  a  lag-lead  control 
law  algorithm  to  calculate  the  joint  servo-valve  control  word.  Joy  stick  control  was  identical  to 
velocity  control  except  that  joy  stick  position  offsets  were  used  as  velocity  set  points.  Vision 
control  utilized  the  centroids  of  an  object,  desired  centroid  locations,  and  a  scheduled  lag-lead 
control  law  algorithm  to  calculate  the  servo-valve  control  words.  Design  of  the  lag-lead  and 
scheduled  lag-lead  control  law  algorithms  can  be  found  in  Pool  (1989).  A  feedback  selector 
determined  which  mode  was  used  to  perform  the  feedback  calculation.  A  numerical  value  of  0 
selected  position  feedback,  a  1  selected  velocity  feedback,  a  2  selected  joy  stick  feedback,  and  a 
3  selected  vision  feedback. 

The  output  drivers  for  joints  0, 1,  and  2  actuators  tested  the  results  calculated  by  the 
lag-lead  compensators  to  ensure  that  the  range  of  the  D/A  converters  was  not  exceeded.  The 
numerical  results  were  then  sent  to  D/A  converters.  The  D/A  converters  generated  a  voltage 
proportional  to  the  result  from  the  lag-lead  compensators.  The  voltage  output  from  the  D/A 
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converters  were  connected  to  servo-amplifier  channels.  The  servo-amplifier  channels  were 
connected  to  servo-valves  and  converted  the  voltage  to  an  amperage  signal.  The  amperage 
signal  was  connected  to  a  coil  in  the  servo-valve  which  displaced  a  servo-valve  spool 
proportionally  to  the  amperage  signal.  Hydraulic  fluid  under  system  pressure  was  directed  to 
an  actuator  through  ports  opened  by  spool  displacement. 
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Figure  4.18  A  joint  output  driver. 


Control  of  the  aperture  opening  (Figure  4.19)  on  the  camera  lens  was  accomplished  using 
a  video  output  channel  on  the  red  image  board.  If  an  object  was  located  in  the  image,  a  fruit 
intensity  set  point  and  the  average  fruit  intensity  were  used  in  the  calculation  of  an  aperture 
opening.  If  an  object  was  not  located,  a  background  set  point  and  the  average  background 
intensity  were  used.  The  appropriate  intensity  level  to  send  to  the  automatic  aperture  was 
calculated  by  utilizing  simple  proportional  control  (Slaughter,  1987). 
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Figure  4.19  The  aperture  output  driver. 


A  power  output  driver  (Figure  4.20)  used  solid  state  relays  to  switch  AC  voltage  to 
different  devices.  The  driver  software  used  three  pieces  of  information  to  determine  the  status 
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of  four  solid  state  relays.  The  picking  device  open  location  was  specified  by  a  numerical  1  and  a 
closed  location  by  a  numerical  2.  A  numerical  value  of  0  removed  power  from  both  picking 
device  solenoids.  Power  to  the  external  push  button  resulted  in  hydraulic  fluid  being  ported  to 
the  robot  if  the  ball  valve  was  open  and  the  external  push  button  was  closed  (refer  to  Figure  4.9 
in  chapter  4  for  the  HPU  dump  valve  circuit).  Power  to  the  external  push  button  was  specified 
by  a  numerical  1  value  and  a  numerical  0  value  removed  power  to  the  external  push  button.  The 
power  supply  for  the  end-effector  lights  was  activated  or  deactivated  using  a  numerical  1  or  0. 
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Figure  4.20  The  power  output  driver. 


Debugging  Facilities 

Debugging  facilities  were  used  to  monitor  activity  during  execution  of  a  robot  program 
and  were  used  to  debug  and  document  activity.  Debugging  facilities  consisted  of  a  data  logger 
and  a  user  interface.  User  interface  facilities  were  used  to  observe  and  make  changes  to 
parameters  during  robot  operation.  Data  logger  facilities  recorded  robot  data  at  a  real-time 
rate  to  document  operation. 

The  user  interface  was  a  command  line  interpreter  that  provided  access  to  important 
robot  data  through  control  room  consoles.  The  user  interface  program  was  run  concurrently  with 
robot  control  programs  but  at  a  lower  execution  priority.  The  user  interface  provided  access  to 
input  and  output  driver  data  and  robot  program  data.  Through  the  user  interface,  the 
programmer  could  alter  data  values  from  a  console  and  display  data  values  on  a  console  screen. 
Two  different  methods  of  data  display  were  used  to  list  data  to  console  screens.  Input  and 
output  driver  data  were  divided  into  related  groups  and  could  be  displayed  in  a  table  or  column 


34 

format.  Display  of  data  to  console  screens  was  not  accomplished  in  real  time  and  the  rate  of 
display  was  dependent  on  the  amount  of  CPU  time  available  to  the  user  interface  program  and 
the  amount  of  data  displayed  to  a  console.  To  observe  data  at  a  60  Hz  rate,  output  had  to  be 
observed  from  the  data  logger  program.  Data  could  be  altered  through  the  user  interface  and 
enabled  developers  to  execute  a  program  using  different  criteria  without  having  to  recompile 
the  program.  This  enhanced  debugging  since  changes  could  be  made  on-line  without  having  to 
make  modifications  to  the  program. 

A  facility  to  save  data  at  a  real-time  rate  was  needed  to  document  and  archive  events 
that  occurred  during  robot  operation.  When  activated,  a  data  logger  program  saved  30  seconds 
of  data  at  a  60  Hz  rate.  The  data  could  be  displayed  to  a  console  screen  or  saved  to  disk  for 
debugging  and  documentation  purposes.  The  data  logger  program  stored  information 
sequentially  into  arrays  until  the  data  arrays  had  been  filled.  The  data  logger  then  sequenced 
back  to  the  beginning  of  the  arrays.  This  resulted  in  a  circular  data  storage  and  provided 
continuous  data  storage  for  the  previous  30  seconds  of  robot  operation. 

Related  groups  of  saved  data  could  be  displayed  to  a  console  screen.  The  display  was 
made  in  a  column  format  similar  to  a  data  display  mode  of  the  user  interface.  The  display 
could  be  interrupted  at  any  time  to  return  to  the  user  interface  program.  All  or  parts  of  the 
saved  data  could  be  transferred  to  the  printer/plotter  work  station  for  graphing.  A  terminal 
emulation  program  operated  on  the  printer/plotter  computer  had  capabilities  to  capture  the 
data  to  a  local  mass  storage  device.  The  data  were  stored  in  a  format  compatible  with  graphics 
software  executed  on  this  computer. 

For  each  set  of  data  recorded  by  the  data  logger,  a  time  stamp  was  recorded  to  indicate 
the  control  cycle  count  and  the  name  of  the  currently  active  state  was  recorded.  The  units  for 
the  time  stamp  were  1/60  second  corresponding  to  the  60  Hz  activation  rate  of  control  software. 
When  data  sets  were  transferred  to  a  mass  storage  device,  the  numerical  value  of  the  first  time 
stamp  was  subtracted  from  the  time  stamp  of  successive  data  sets  to  establish  a  zero  time  base 
for  the  data  sets. 


The  data  logger  also  marked  certain  data  locations  for  documentation.  This  feature 
was  used  to  document  data  values  that  did  not  change  in  real  time.  The  user  could  send 
configuration  information  to  the  printer/ plotter  computer  to  document  the  values  of  marked 
data.  Storage  of  this  information  established  documentation  capabilities  for  numerical  data 
such  as  joint  minimum  and  maximum  position  and  velocity  limits. 


CHAPTER  5 

DEFINITION  OF  THE  PROGRAMMING  LANGUAGE 


This  chapter  describes  the  programming  language  developed  to  control  a  vision- 
servoed,  fruit-picking  robot.  The  picking  language  consisted  of  three  elements:  states,  models, 
and  parameter  definitions.  The  state  language  structure  specified  desired  robot  actions.  The 
model  language  structure  combined  sensor  and  system  information  to  detect  the  status  of  an 
important  event.  States  were  linked  together  into  a  state  network  with  logical  combinations  of 
modeling  results.  A  programming  environment  consisting  of  a  C  programming  language  compiler 
and  a  text  editor  was  used  to  develop  and  compile  a  robot  program.  An  editor  and  a  compiler  for 
the  picking  language  were  not  developed  in  this  work.  The  robot  program  was  executed  in  an 
operating  environment  shown  in  Figure  5.1.  The  operating  environment  included  input  and 
output  hardware  and  software,  debugging  facilities,  and  a  real-time  kernel  to  schedule 
activation  of  software  components.  Central  to  the  operating  environment  was  a  data  base 
structure  which  provided  for  asynchronous  data  exchange  among  different  software  components. 
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Figure  5.1  Layout  of  system  components. 
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This  chapter  begins  by  defining  the  structure  of  the  data  base  and  describes  the  input 
and  output  data  structures  required  by  the  picking  language.  Elements  of  the  picking  language 
are  then  described.  A  formal  definition  of  the  language  syntax  (for  example,  using  Backus- 
Naur  form)  is  not  presented  in  this  work.  The  programming  methodology  is  defined  and 
examples  are  given  to  support  the  concepts  of  program  development.  A  description  of  the 
scheduler — the  real-time  kernel  responsible  for  orderly  execution  of  software  elements — is  then 
presented.  This  chapter  concludes  with  a  simple  example  of  a  task-level  program  developed 
with  the  picking  language. 

Data  Base  Structures 

Intelligent  operation  of  a  robot  required  the  ability  to  sense  the  environment,  make 
intelligent  decisions,  and  output  information  to  actuation  devices.  Since  input  and  output 
capabilities  were  hardware  dependent,  a  method  of  data  exchange  was  necessary  between 
input  and  output  driver  software  and  the  programming  language.  This  was  accomplished  by 
reserving  a  section  of  computer  memory  for  storage  of  global  information.  Input  hardware  and 
driver  software  (described  in  chapter  4)  were  utilized  to  acquire  sensor  information  from  the 
environment.  Output  hardware  and  driver  software  (also  described  in  chapter  4)  calculated 
and  sent  data  base  information  to  actuation  devices.  Both  input  and  output  driver  software 
utilized  sections  of  the  data  base  memory  to  store  information. 

The  amount  of  computer  memory  required  to  store  information  was  dependent  on  the 
numerical  size  and  amount  of  data  required  by  driver  software.  The  fundamental  data  types 
were  flags,  characters,  integers,  and  floating  points.  The  data  types  and  amount  of  data  base 
memory  required  for  storage  are  shown  in  Table  5.1. 

The  picking  language  required  input  and  output  driver  data  be  represented  in  the  data 
base  structure  in  the  structure  shown  in  Figure  5.2.  The  top  section  presented  the  name  and 
function  of  the  driver  software.  The  next  section  contained  a  parameter  list  where  each  row 
represented  a  data  element  in  the  data  base.  The  number  of  rows  varied  according  to  the  number 
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Table  5.1  Data  types  and  sizes. 


Type 

Size  (bits) 

Comment 

flag 

8 

TRUE  or  FALSE  Boolean  value 

char 

8 

a  single  byte  that  holds  one  character 

int 

16 

an  integer 

long  int 

32 

a  long  integer 

double 

64 

a  double  precision  floating  point  number 

Name: 
Function: 

Name 

Field 

Type 

Range 

Log 

Comment 

Figure  5.2  Data  base  structure  for  input  and  output  drivers. 


of  parameters  accessible  to  the  picking  language.  The  name  by  which  the  parameter  was 
referred  in  the  data  base  was  listed  in  the  Name  column.  If  an  array  of  values  was  used  by 
driver  software,  brackets  ([])  were  placed  after  the  parameter  name  and  the  number  of  array 
elements  was  specified  inside  the  brackets.  A  Comment  column  provided  a  brief  explanation  of 
the  meaning  or  use  of  the  parameter.  The  Field  column  contained  a  label  that  indicated 
parameter  use.  Examples  of  parameter  fields  included  a  result  from  an  input  driver  or  a  set 
point  for  an  output  driver.  The  Type  column  specified  the  data  type  allocated  for  storage  (see 
Table  5.1  for  defined  data  types).  The  Range  column  provided  a  limitation  on  the  numerical 
value  that  could  be  assigned  to  a  parameter.  The  contents  of  the  Range  column  were  used  by  the 
real-time  kernel  and  the  user  interface  to  ensure  data  integrity.  The  user  interface  program 
utilized  information  in  the  Range  column  to  validate  on-line  changes  before  altering  a 
parameter  value  in  the  data  base.  Parameter  limits  were  established  using  constructs  from  the 
C  programming  language  (Kernighan  and  Ritchie,  1978).  A  summary  of  symbols  used  in  range 
specification  is  given  in  Table  5.2. 

A  range  was  specified  by  entering  a  relational  operator  followed  by  an  expression. 
Pairs  of  operator— expression  could  be  concatenated  with  logical  connective  operators.  If  a 
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Table  5.2  Symbolic  notations  from  the  C  programming  language  used  in  the  picking  language. 


Symbol 

Operator 

Comment 

+ 

arithmetic 

addition 

- 

arithmetic 

subtraction 

* 

arithmetic 

multiplication 

/ 

arithmetic 

division 

unary 

unary  negation 

J 

unary 

logical  negation 

? : 

ternary 

used  in  constructing  conditional  statements  in  an  expression 

> 

relational 

greater  than 

>= 

relational 

greater  than  or  equal  to 

< 

relational 

less  than 

<= 

relational 

less  than  or  equal  to 

relational 

equal  to 

i= 

relational 

not  equal  to 

&& 

logical  connective 

logical  and 

II 

logical  connective 

logical  or 

assignment 

value  of  expression  to  right  of  =  is  assigned  to  parameter  on  left 

relational  operator  was  not  explicitly  given,  the  equal  to  (==)  relational  operator  was 

assumed.  Expressions  were  generally  a  number  or  a  parameter  name  that  evaluated  to  a 

numerical  value.  An  example  of  a  range  specification  for  a  joint  position  set  point  (pOsp)  is 

>=-30.0  &&<=30.0. 

A  natural  language  interpretation  of  the  range  specification  is 

IF((pOsp  >=  -30.0)  &&  (pOsp  <=  30.0)) 

THEN  position  set  point  is  within  range  specification 

ELSE  position  set  point  is  outside  range  specification. 

The  Log  column  defined  data  logging  action.  The  data  logger  recorded  selected  portions 

of  the  data  base  at  a  real-time  rate  and  marked  parameters  in  the  data  base  for  documentation. 

An  entry  of  I  in  the  Log  column  indicated  that  the  parameter  was  recorded  by  the  data  logger  at 

a  60  Hz  rate.  An  entry  of  c  indicated  that  the  parameter  was  marked  for  inclusion  in  a 

configuration  file.  The  configuration  file  was  utilized  to  document  parameter  values  that  did 

not  change  at  a  real-time  rate.  Examples  of  parameter  values  included  in  the  configuration  file 

were  joint  minimum  and  maximum  position  limits.  If  the  Log  column  cell  was  blank,  the  data 

logger  did  not  take  any  action  regarding  the  parameter.  Data  recorded  at  a  real-time  rate  and 

configuration  data  could  be  listed  to  a  console  screen  for  on-line  debugging  or  transferred  to  the 
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printer/plotter  console  to  be  stored  on  a  mass  storage  device.  When  the  configuration  file  was 
displayed  to  a  console  screen  or  transferred  to  the  printer/ plotter  console,  the  current  values  of 
the  parameters  marked  for  configuration  were  listed. 

Data  Base  Structure  for  Input  Drivers 

Input  drivers  acquired  data  from  a  sensor  device,  converted  data  to  meaningful  results, 
and  stored  results  in  the  data  base.  Input  drivers  consisted  of  the  hardware  (primary  sensing 
elements  and  the  computer  interface  components)  and  software  to  perform  these  actions.  The 
software  for  an  input  driver  initialized  hardware,  acquired  sensor  information,  converted,  and 
stored  data  in  the  data  base.  To  the  programmer,  the  important  aspects  of  input  driver 
software  were  the  results  stored  in  the  data  base.  The  method  of  data  acquisition  and 
processing  were  of  no  concern  to  the  programmer. 

Input  drivers  had  one  data  base  field — result.  Input  drivers  stored  processed  numerical 
data  in  result  parameters.  Result  fields  could  only  be  modified  by  the  input  driver  and  were 
read-only  for  the  user  interface  and  robot  program.  A  range  provided  with  a  result  parameter 
defined  information  to  the  programmer  and  was  used  for  error  checking  by  input  driver 
software. 

An  example  of  the  data  base  structure  for  an  input  driver  used  to  quantify  joint  position 
and  velocity  is  presented  in  Figure  5.3.  This  is  the  information  provided  to  the  picking 
language  for  position  and  velocity  measurements  of  joint  0.  Figures  4.10  and  4.11  in  chapter  4 
show  the  hardware  and  software  operations  for  the  input  driver.  The  input  driver  calculated 
two  results  and  stored  these  in  the  data  base;  pO  and  vO  were  the  data  base  storage  locations  for 
the  position  and  velocity  of  joint  0  respectively.  The  entries  in  the  Range  column  provided 
information  about  the  expected  values  of  joint  position  and  velocity.  The  range  of  joint  0's 
position  was  ±35  degrees.  The  range  of  joint  0's  velocity  was  ±150  degrees  per  second.  The  I  in 
the  Log  column  indicated  that  joint  position  and  velocity  was  recorded  at  a  real-time  rate  by 
the  data  logger. 
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Name: 

Joint  0  position  and  velocity  input  driver. 

Function:  Obtain  the  position  and  velocity  of  joint  0. 

Name 

Field 

Type 

Range 

Log 

Comment 

pO 

result 

double 

<  35.0  &&  >  -35.0 

1 

position  of  joint  0  (degrees) 

vO 

result 

double 

<  150.0  &&  >  -150.0 

1 

velocity  of  joint  0  (degrees/ second) 

Figure  5.3  Data  base  structure  for  joint  0  position  and  velocity  input  driver. 
Data  Base  Structure  for  Output  Drivers 


Output  driver  software  calculated  and  transferred  information  to  an  actuation  device. 
Output  driver  hardware  included  the  primary  actuation  elements  and  the  computer  interface 
components  necessary  to  deliver  the  information.  Output  driver  software  was  classified  as 
either  closed-loop  or  open-loop  according  to  the  type  of  control  needed  for  the  device.  Closed- 
loop  output  driver  software  calculated  a  value  to  output  according  to  a  feedback  control  law 
algorithm.  The  establishment  of  more  than  one  feedback  mode  allowed  the  programmer  to 
specify  the  feedback  mode  for  the  actuation  device.  Open-loop  output  driver  software  was  used 
to  control  a  device  that  did  not  require  feedback. 

In  addition  to  the  result  field,  two  additional  fields — activation  and  set  point — were 
defined  for  data  base  structures  for  output  driver  software.  If  a  closed-loop  output  driver  had 
multiple  feedback  modes,  an  activation  parameter  specified  which  control  mode  was  used  to 
calculate  the  feedback  value.  This  allowed  different  feedback  modes  or  laws  to  be 
implemented  during  execution  of  a  robot  program.  Examples  of  different  feedback  modes  include 
position  and  velocity  feedback.  Set  point  parameters  were  used  in  closed-loop  output  drivers  to 
establish  set  points  for  feedback  compensators.  Open-loop  output  driver  software  used 
activation  parameters  to  establish  the  desired  status  of  open-loop  actuation  devices. 

An  example  of  an  output  driver  was  the  hardware  and  software  necessary  to  control  the 
position  of  a  servo-hydraulically  actuated  robot  joint  (see  Figure  4.18  in  chapter  4).  The  data 
base  structure  for  an  output  driver  used  to  control  a  robot  joint  (joint  0)  is  given  in  Figure  5.4.  In 
this  example,  two  feedback  modes  were  available  to  calculate  a  result  to  transfer  to  the 
actuation  device.  Position  feedback  control  used  the  current  joint  position  (input  driver  result), 
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a  position  set  point,  and  an  unspecified  feedback  compensator  to  compute  a  command  (control 
word)  to  send  to  the  actuator.  Velocity  feedback  control  used  the  current  joint  velocity,  a 
velocity  set  point,  and  an  unspecified  feedback  compensator  to  compute  a  command  to  send  to 
the  actuator  driver.  The  jointOctl  activation  parameter  was  used  to  specify  which  feedback 
mode  was  used  to  calculate  the  result.  If  the  activation  parameter  evaluated  to  zero  (0), 
position  feedback  was  used  to  calculate  the  servo-valve  control  word.  If  the  activation 
parameter  evaluated  to  one  (1),  velocity  feedback  was  used  to  calculate  the  servo-valve  control 
word. 


Name:     Joint  0  position  output  driver. 

Function:  Calculate  and  output  a  control  value  to  D/A  converter  connected  to  servo-amplifier. 
Control  modes:  0  ■  position  feedback,  1  =  velocity  feedback. 

Name 

Field 

Type 

Range 

Log 

Comment 

vOcw 

result 

int 

<  2047  &&  >  -2047 

1 

joint  0  servo-valve  control  word 

jointOctl 

activation 

int 

0111 

1 

feedback  mode 

pOsp 

set  point 

double 

<  35.0  &&  >  -35.0 

1 

joint  0  position  set  point 

vOsp 

set  point 

double 

<  150.0  &&> -150.0 

1 

joint  0  velocity  set  point 

Figure  5.4  Data  base  structure  for  joint  0  output  driver. 


The  operations  of  the  joint  output  drivers  are  not  shown  in  the  data  base  structure. 
Again,  it  was  not  important  for  the  programmer  to  understand  how  joint  control  information  was 
calculated  and  sent  to  the  actuation  driver.  The  important  feature  was  that  feedback  control 
was  implemented  by  setting  jointOctl  to  either  0  or  1  according  to  the  desired  feedback  mode  and 
establishing  the  appropriate  control  set  point.  The  set  points  could  be  altered  by  a  robot 
program  or  through  the  user  interface  between  minimum  and  maximum  position  limits  given  in 
the  Range  column.  The  I  in  every  cell  in  the  Log  column  indicated  each  parameter  was  recorded 
by  the  data  logger  at  the  completion  of  each  control  cycle. 

Picking  Language 

The  components  of  the  picking  language  were  states,  models,  and  global  parameter 
definition.  The  functions  of  states  were  tri-fold.  Each  state  specified  actions  to  be  performed, 
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determined  when  the  actions  had  been  completed  or  a  problem  existed,  and  specified  the  next 
state  to  activate.  Models  reduced  input  and  output  driver  and  system  data  to  binary  values  for 
use  in  determining  when  actions  had  been  completed  or  when  problems  existed.  Global 
parameters  defined  in  the  data  base  were  used  for  communication  between  the  user  interface 
and  a  robot  program  and  storage  of  information  between  control  cycles. 

The  concept  for  programming  a  task  using  the  picking  language  is  shown  graphically  in 
Figure  5.5.  Action  specifications  were  shown  as  rounded  rectangles  and  represented 
modifications  made  to  the  data  base.  Lines  with  arrows  were  links  between  action  steps  and 
represented  decisions  as  to  when  the  next  state  was  executed.  A  single  state  constituted  the 
action  specifications  within  a  block  and  the  exit  links.  Only  one  state  was  active  at  a  given 
time.  The  remaining  states  were  in  an  inactive  mode.  A  state  was  placed  in  the  active  mode  by 
making  modifications  to  the  data  base  according  to  the  actions  desired  to  be  completed  during 
the  state's  activation.  The  data  base  modifications  included  specification  of  the  feedback 
control  mode  (if  multiple  feedback  modes  were  defined  for  an  output  driver)  and  establishing 
control  set  points.  After  modifications  to  the  data  base  were  made,  the  state  was  placed  in  the 
active  mode.  While  the  state  was  active,  links  to  other  states  were  evaluated  each  control 
cycle  to  determine  when  the  next  state  should  be  activated. 


Figure  5.5  Programming  concept  for  picking  language. 

State  links  evaluated  the  status  of  global  parameters  and  modeling  results  and  directed 
program  flow.  In  Figure  5.5,  state  A  was  linked  to  state  B  by  link  Al  and  to  state  C  by  link  A2. 
The  links  were  numbered  to  indicate  the  order  of  evaluation.  While  the  actions  specified  by 
state  A  were  performed,  links  Al  and  A2  were  sequentially  evaluated.  State  A  remained 
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active  if  both  links  evaluated  false.  If  one  link  evaluated  true,  state  A  was  deactivated  and 
the  state  pointed  to  by  the  link  was  activated.  The  ordering  of  the  links  was  important  since 
the  remaining  links  were  not  evaluated  when  one  link  evaluated  true.  Thus,  if  both  links  Al 
and  A2  would  evaluate  true,  state  B  was  activated  since  link  Al  was  evaluated  before  link  A2. 

A  state  was  deactivated  by  returning  modeling  criteria  that  had  been  altered  during 
state  initialization  to  criteria  values  utilized  before  the  state  was  activated.  This  allowed  a 
state  to  alter  modeling  criteria  for  the  duration  of  a  state's  activation  but  prohibited  global 
changes  in  modeling  criteria.  After  modeling  criteria  had  been  reset,  the  state  associated  with 
the  true  link  was  activated  by  making  its  modifications  to  the  data  base. 

Changes  in  the  task  program  were  accomplished  by  the  addition  of  new  states  and 
linking  existing  states  to  the  new  states.  If  the  programmer  observed  a  problem  during  the 
execution  of  a  state,  additional  states  could  be  added  to  solve  the  problem.  This  concept  is 
shown  in  Figure  5.6  with  the  addition  of  state  E  and  link  A3.  Link  A3  determined  if  the 
problem  occurred  while  state  A  was  active.  Links  from  state  E  to  the  existing  states  C  and  D 
continued  execution  of  the  program  after  the  problem  had  been  solved  by  state  E.  No  changes  in 
states  C  or  D  had  to  be  made  to  recognize  the  new  links  from  state  E.  Of  the  existing  states  in 
the  program,  only  state  A  had  to  be  altered  by  inclusion  of  an  additional  link.  This  feature 
allowed  alterations  to  be  made  with  minimal  modification  to  an  existing  program.  If  more 
than  one  state  was  necessary  for  problem  solution,  additional  states  could  be  added  and  linked 
to  the  existing  program  with  the  same  ease.  The  collection  of  states  was  called  the  state 
network  because  of  the  connectivity  of  individual  states. 


Figure  5.6  Addition  of  a  state  to  the  program. 
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Models 

Models  characterized  events  occurring  in  the  environment  by  utilizing  modeling  criteria 

to  map  robot  data  to  binary  results.  Although  input  drivers  mapped  infinitely  variable 

environmental  quantities  to  finite  numerical  results,  the  data  sets  were  too  large  to  be  handled 

efficiently  in  state  links.  It  was  easier  and  more  uniform  to  program  state  links  by  utilizing 

results  from  models.  A  natural  language  version  of  a  model  to  determined  if  a  joint  position  (pO) 

was  outside  defined  minimum  (pOmin)  or  maximum  (pOmax)  position  limits  is 

IF  (pO  is  greater  than  pOmax)  OR  (pO  is  less  than  pOmin) 

THEN  joint  position  is  out  of  position  limits 

ELSE    joint  position  is  within  maximum  and  minimum  limits. 

All  models  had  two  elements:  modeling  logic  and  model  parameters.  A  model  was 
constructed  by  symbolically  defining  these  elements  in  a  model  programming  form  (Figure  5.7). 
This  form  was  similar  to  the  data  base  structure  used  for  input  and  output  drivers.  The  upper 
portion  of  the  model  programming  form  provided  space  for  the  programmer  to  specify  a  model 
name  and  function.  The  middle  section  contained  rows  of  seven  columns  for  parameter 
definition.  The  functions  of  the  Name,  Field,  Type,  Range,  Log,  and  Comment  columns  were 
identical  to  the  definition  of  driver  data  base  structures.  The  parameter  fields  defined  for 
models  were  result,  set  point,  and  storage.  Result  parameters  were  stored  in  the  data  base  and 
held  the  Boolean  result  of  the  modeling  logic.  The  result  field  was  restricted  to  the  type  flag 
and  each  model  had  at  least  one  result  parameter.  Modeling  criteria  were  stored  in  set  point 
parameters.  Storage  parameters  were  used  to  hold  local  model  information  between  control 
cycles  and  were  not  stored  in  the  data  base.  A  blank  entry  in  the  Range  column  signified  that 
the  value  of  a  parameter  could  not  be  altered  through  the  state  network  or  through  the  user 
interface.  The  Initial  column  was  used  to  specify  a  value  that  was  assigned  to  the  parameter 
during  system  initialization. 

The  modeling  logic  was  entered  by  specifying  modeling  execution  statements  using 
constructs  from  the  C  programming  language  (Kernighan  and  Ritchie,  1978).  A  summary  of  the 
symbols  used  for  specify  modeling  logic  was  given  in  Table  5.2.  Data  manipulation  on  storage 
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Model  Name: 

Function: 

Name 

Field 

Type 

Range 

Initial 

Log 

Comment 

result 

flag 

TRUE  II  FALSE 

Logic: 

Figure  5.7  Model  programming  form. 


parameters  were  performed  according  to  the  function  of  the  model.  In  general,  models 

performed  comparison  tests  between  information  in  the  data  base  and  modeling  set  points.  A 

result  parameter  was  set  to  true  or  false  according  to  the  result  of  the  comparison.  The 

comparison  test  was  made  using  the  form: 

if(model_logic)   model_result  =  TRUE; 
else  model_result  =  FALSE; 

where  model_logic  was  an  expression  that  evaluated  to  true  or  false.  Storage  parameters  were 

altered  using  the  form: 

parameter_name  =  expression; 
where  parameter_name  was  a  storage  parameter  defined  by  the  model  and  expression 
evaluated  to  a  value  consistent  with  the  data  type  of  parameter_name.  The  only  data  in  the 
data  base  that  a  model  could  alter  were  its  result  parameters.  A  model  could  not  alter  any 
other  data  stored  in  the  data  base.  The  information  given  in  the  model  form  was  used  by  a 
compiler  to  include  the  modeling  feature  in  the  control  cycle  execution  sequence.  The  joint 
position  out-of-limits  example  is  shown  in  Figure  5.8  to  illustrate  model  definition. 

Macro  Substitution 

When  programming  modeling  logic,  macros  provided  a  consistent  method  to  establish 
constants  and  in-line  code.  A  macro  substitution  list  was  used  to  replace  a  name  with  a  string  of 
characters.  Definitions  were  similar  to  the  #define  construct  in  the  C  language.  Macro 
definitions  were  made  by  specifying  information  in  a  form  shown  in  Figure  5.9.  The  name  to  be 
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Model  Name:  Joint  0  out-of-limits. 

Function: 

Determine  when  the  position  of  joint  0  is  above  or  below  position  limits.  If 

position  of  joint  0  is  below  minimum  or  above  maximum  position  limits,  set 

fpOool  to  true.  Set  fpOool  to  false  if  the  position  of  joint  0  is  within  minimum 

and  maximum  position  limits. 

Name 

Field 

Type 

Range 

Initial 

Log 

Comment 

fpOool 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  0  out  of  position  limits 

pOmax 

set  point 

double 

>=  0.0  &&  <=  35.0 

35.0 

1 

maximum  position  limit 

pOmin 

set  point 

double 

<=0.0  &&  >=-35.0 

-35.0 

1 

minimum  position  limit 

Logic:  if(  (pO  >  pOmax)  II  (pO  <  pOmin) )           fpOool  =  TRUE; 

else 

fpOool  =  FALSE; 

Figure  5.8  Model  definition  for  joint  0  out-of-position  limits. 


Macro  Definitions. 

Name 

Replacement  String 

Comment 

TRUE 

1 

Boolean  true  value 

FALSE 

0 

Boolean  false  value 

ABS(x) 

((x)<0.0>?  (-(x))  :  (x) 

Absolute  value  of  expression  x 

Figure  5.9  Macro  definition  list. 


replaced  with  the  character  string  was  entered  in  the  Name  column.  The  replacement 
character  string  was  entered  in  the  Replacement  String  column.  A  Comment  column  provided 
space  for  explanation  of  the  macro.  Three  macros  were  defined  in  this  figure.  The  first  two 
indicate  that  the  character  string  TRUE  was  replaced  with  a  1  and  FALSE  was  replaced  with  a 
0  by  a  compiler.  The  ABS(x)  macro  was  used  to  calculate  the  absolute  value  of  the  expression 
enclosed  within  the  parentheses  and  used  the  ternary  operator  (?  :)  to  perform  a  conditional  test 
on  the  expression  x.  The  expression  (x)  was  evaluated  and  the  value  was  compared  to  zero.  If 
the  value  of  the  expression  was  less  than  zero,  the  value  was  negated  to  form  a  positive  result. 

States  and  the  State  Network 

States  defined  actions  to  perform,  determined  when  the  actions  had  been  accomplished 
or  problems  existed,  and  indicated  the  next  state  to  activate.  To  develop  a  robot  program,  a 
state  planning  form  and  a  state  network  map  had  to  be  completed.  The  state  planning  form 
(Figure  5.10)  consisted  of  three  columns.  The  state  name  (generally  indicative  of  the  state 
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function)  was  entered  in  the  State  Name  column.  The  state  activated  during  system 
initialization  was  the  start_up  state.  The  start_up  state  name  was  reserved  for  this  use.  The 
Alias  Names  column  was  used  to  assign  different  names  to  a  state  for  debugging  purposes.  A 
state  link  to  one  of  the  alias  names  had  the  effect  of  actuating  the  parent  state  but  with  the 
alias  name.  This  provided  a  method  of  documenting  the  reason  a  state  was  activated  if 
multiple  links  to  a  state  existed.  For  example,  if  an  error  was  detected  by  a  state  and  used  an 
alias  name  (indicative  of  the  type  of  error  detected)  to  activate  the  error  handling  state,  the 
reason  the  error  handling  state  was  activated  could  be  determined  by  observing  the  state 
activation  sequence.  The  Comment  column  was  used  to  describe  the  actions  performed  by  the 
state. 


State  Name 

Alias  Names 

Comment 

start_up 

state#l 

state#2 

state#3 

state#4 

Figure  5.10  State  planning  form. 


A  state  network  map  was  used  to  program  initialization  statements  and  state  links. 
The  state  network  map  shown  in  Figure  5.11  includes  the  output  driver  (Figure  5.4)  and  model 
(Figure  5.8)  examples.  The  state  network  map  consisted  of  two  sections — initialization  and 
state  link.  The  first  group  of  rows  was  the  initialization  section  and  the  second  group  of  rows 
was  the  state  linking  section.  The  first  column  in  the  initialization  section  contained  a  listing 
of  data  base  parameters  that  could  be  altered  by  states.  The  parameters  that  a  state  could 
alter  included  activation,  set  point,  and  system  parameters  from  data  base  structures  of  output 
drivers,  models,  and  global  parameter  definition.  The  first  column  in  the  state  linking  section 
contained  a  listing  of  the  states  and  state  alias  names  defined  by  the  programmer.  Each 
remaining  column  in  the  state  network  map  was  used  to  program  individual  states. 
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TniH3li7aHon 

1111  UUllliU  11  \J  1  1 

state#l 

state#2 

state#3 

state#4 

iointOctl 

IK'  III  U/v.  ^  1 

pOsp 

vOsp 

pOmin 

pOmax 

State  Links 

start_up 

state#l 

state#2 

state#3 

state#4 

start_up 

state#l 

statc#2 

state#3 

state#4 

Figure  5.11  State  network  map. 


State  actions  were  defined  by  specifying  changes  to  parameters  in  the  data  base  when  a 
state  was  activated.  A  change  in  a  data  base  parameter  was  programmed  by  entering  an 
expression  at  the  intersection  of  the  parameter  row  (listed  in  the  first  column)  and  the  state 
being  programmed.  If  the  intersection  of  a  parameter  row  and  a  state  column  was  left  blank, 
that  parameter  was  not  altered  during  initialization  of  the  state.  If  an  expression  was  entered 
in  the  cell,  the  expression  was  evaluated  during  state  initialization  and  the  value  of  the 
parameter  in  the  data  base  was  changed.  The  value  of  activation  parameters  and  the 
appropriate  set  point  parameters  used  by  output  drivers  had  to  be  established  for  each  state. 
When  a  state  was  deactivated,  model  set  point  parameters  that  had  been  changed  during  state 
initialization  were  restored  to  pre-state  values.  For  example,  if  a  state  altered  the  minimum 
position  limit  during  state  initialization,  the  changed  value  for  the  position  limit  was  valid 
only  during  that  state's  activation.  When  the  state  was  deactivated,  the  minimum  position 
limit  was  restored  to  its  value  prior  to  state  activation.  This  prevented  a  state  from  making 
permanent  changes  to  modeling  criteria. 

The  logic  linking  one  state  to  another  state  was  programmed  in  the  state  linking 
section.  Logic  expressions  were  entered  at  the  intersection  of  a  state  column  and  rows  in  the 
state  linking  section.  A  state  (name  listed  at  the  top  of  a  column)  was  linked  to  another  state 
(listed  in  a  row)  if  an  expression  was  entered  in  the  intersecting  row  and  column  cell.  The  state 


50 

links  had  the  form:  #.  expression.  The  number  (#.)  specified  the  execution  priority  of  the  link. 
The  expression  had  to  evaluate  to  either  true  or  false.  An  example  of  a  state  link  is 

1.  fpOool. 

The  execution  number  (1.)  indicated  that  this  state  link  was  evaluated  first.  The  expression 
(fpOool)  used  a  result  calculated  by  an  out-of-limits  model.  If  fpOool  evaluated  true,  the  state 
associated  with  the  state  link  was  activated.  Logical  connectives  (  &&  and  II)  were  used  to  form 
more  complex  logic  in  programming  state  links. 

Safety  Links 

One  important  aspect  of  robot  operation  was  safety.  To  maintain  system  integrity 
during  operation  of  the  robot,  the  existence  of  errors  or  fault  conditions  had  to  be  detected  every 
control  cycle.  To  relieve  the  programmer  from  having  to  explicitly  program  safety  features  in 
every  state,  global  linking  capabilities  were  provided.  Links  identified  as  safety  links  were 
evaluated  every  control  cycle  prior  to  evaluation  of  the  active  state's  explicit  links.  Safety 
links  were  identical  in  every  aspect  to  ordinary  state  links  except  for  this  global  evaluation 
and  implicit  nature.  This  provided  a  safety  feature  that  was  applied  to  all  states  with  one 
definition.  Safety  links  were  added  to  the  state  network  by  completing  the  State  Link,  Link 
Logic,  and  Comment  columns  in  the  form  shown  in  Figure  5.12. 


Safety  Links 

State  Link 

Link  Logic 

Comment 

1. 

2. 

Figure  5.12  Safety  link  programming  form. 


Global  Parameter  Definitions 

Global  parameters  provided  communication  between  the  user  interface  and  the  robot 
program  and  within  the  robot  program  itself.  Two  parameter  fields,  assignment  and  system, 
were  available  to  the  programmer  for  these  purposes.  The  numerical  value  of  assignment  and 
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system  parameters  could  be  altered  through  the  user  interface.  Assignment  parameters  were 
used  by  the  programmer  during  state  definition  to  establish  changes  in  set  point  parameters. 
When  a  state  was  activated,  the  value  of  the  assignment  parameter  was  used  to  alter  a  set 
point  parameter.  Since  the  value  of  assignment  parameters  could  be  altered  on-line  through  a 
user  interface,  the  run-time  value  of  set  point  parameters  used  during  the  activation  of  a  state 
could  be  varied. 

Assignment  parameters  were  defined  using  a  data  base  structure  shown  in  Figure  5.13. 
The  Name,  Range,  Initial,  Log,  and  Comment  columns  were  identical  to  the  data  base  structure 
for  model  definition.  A  Field  column  was  not  necessary  since  this  data  base  structure  was  used 
exclusively  to  define  assignment  parameter.  The  Assign  to  Parameter  column  replaced  the  Type 
column  and  was  used  to  indicate  which  parameter  (either  output  driver  or  modeling  set  point) 
could  be  altered  to  the  value  of  the  assignment  parameter.  An  assignment  parameter  could  only 
be  used  to  alter  that  specified  set  point  parameter  during  state  initialization.  The  Type  column 
was  not  necessary  because  each  assignment  parameter  was  associated  with  one  set  point 
parameter  and  the  type  had  to  be  consistent  with  the  associated  set  point  parameter. 
Assignment  parameters  could  not  be  altered  by  a  state  and  were  not  listed  in  the  initialization 
section  of  a  state  network  map.  Assignment  parameters  could  be  altered  through  the  user 
interface.  The  range  specification  given  in  the  assignment  parameter  data  base  structure  and 
the  range  specification  of  the  set  point  parameter  were  used  to  validate  numerical  changes  of 
assignment  parameters  before  storage  in  the  data  base.  The  user  interface  did  not  make  the 
change  if  the  numerical  value  exceeded  either  range  specification.  This  prevented  a 
programmer  from  defining  a  range  for  an  assignment  parameter  greater  than  the  range  for  the 
set  point  parameter. 

System  parameters  were  used  for  communication  between  the  robot  program  and  the  user 
interface  and  for  storage  of  information  between  control  cycles.  System  parameters  were  used 
primarily  as  communication  flags  when  the  program  waited  for  a  response  from  the  user.  For 
example,  a  program  may  have  different  state  sequences  programmed  in  the  state  network  and 


52 


Assignment  Parameters 

Name 

Assign  to 
Parameter 

Range 

Initial 

Log 

Comment 

Figure  5.13  Data  base  structure  for  assignment  parameter  definition. 


one  state  acts  as  a  central  branching  node.  The  state  links  from  the  branching  state  evaluated 
the  status  of  system  parameters  to  determine  which  state  sequence  was  performed.  System 
parameters  were  added  to  the  data  base  by  defining  a  variable  in  the  data  base  structure  form 
shown  in  Figure  5.14.  The  parameter  definition  columns  were  identical  to  the  data  base 
structure  used  to  program  models  with  the  exception  that  a  Field  column  was  not  necessary 
because  this  form  was  used  exclusively  for  system  parameter  definition.  System  parameters 
could  be  altered  through  the  user  interface  and  during  state  initialization.  System  parameters 
were  listed  in  the  state  network  map  initialization  section  and  were  not  reset  during  state 
deactivation. 


System  Parameters 

Name 

Type 

Range 

Initial 

Log 

Comment 

Figure  5.14  Data  base  structure  for  system  parameter  definition. 


Scheduler 

The  scheduler  was  a  real-time  kernel  which  synchronized  execution  of  software 
components.  The  operation  sequence  of  the  scheduler  is  shown  in  Figure  5.15.  System  start-up 
code  represented  loading  executable  code  into  computer  memory  and  assigning  priority  levels  to 
different  software  components.  The  data  logger  program  was  assigned  an  execution  priority 
level  below  that  of  the  scheduler  but  above  the  execution  priority  level  of  user  interfaces.  The 
system  initialization  block  executed  initialization  components  of  input  and  output  drivers  to 
initialize  driver  hardware,  initialized  data  base  parameters  to  initial  values,  and  activated 
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deactivate 
current  state 


activate 
next  state 


Figure  5.15  Real-time  kernel. 


the  start_up  state.  After  system  initialization,  the  execution  of  the  scheduler  program  was 
suspended  until  a  60  Hz  activation  signal  concurrent  with  the  acquisition  of  a  new  video  held 
was  received.  After  the  activation  signal  was  received,  a  series  of  actions  were  performed  and 
program  operation  suspended  until  an  activation  signal  was  received  again.  The  cycle  from 
activation  to  suspension  of  the  scheduler  was  called  a  control  cycle. 

When  the  activation  signal  was  received,  the  scheduler  incremented  a  system 
parameter  that  was  used  to  time  how  long  a  state  had  been  active.  The  scheduler  then  updated 
sensor  information  in  the  data  base  by  activating  input  drivers.  After  sensor  information  had 
been  updated,  models  defined  by  the  programmer  were  executed  and  modeling  results  stored  in 
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the  data  base.  Safety  links  were  then  evaluated  to  determine  system  integrity.  If  a  safety  link 
evaluated  true,  the  currently  active  state  was  deactivated  and  the  state  associated  with  the 
true  safety  link  was  activated.  When  a  state  was  deactivated,  the  scheduler  added  how  long 
the  state  had  been  active  to  the  state's  running  total.  The  running  total  indicated  the  total 
time  that  a  state  had  been  active. 

If  every  safety  link  evaluated  false,  the  state  links  of  the  active  state  were  evaluated. 
If  a  state  link  evaluated  true,  the  currently  active  state  was  deactivated  by  restoring  changes 
to  modeling  criteria  to  the  value  the  modeling  criteria  had  before  the  state  was  activated.  The 
next  state  was  initialized  by  making  its  specified  changes  in  the  data  base.  If  all  of  the  state 
links  evaluated  false  (or  after  a  state  switch  was  made),  the  output  drives  were  executed  to 
calculate  and  transfer  data  to  actuation  devices.  The  scheduler  then  examined  the  activation 
status  of  the  data  logger.  If  the  data  logger  was  set  to  record  data,  logging  activities  were 
activated.  Scheduler  operation  was  then  suspended  to  wait  for  the  next  activation  signal. 

Programming  Example 
An  example  is  now  given  to  illustrate  how  a  robot  program  is  developed  using  the 
picking  language.  This  example  describes  the  development  of  the  states  and  models  necessary 
to  control  one  robot  joint.  The  concept  of  the  program  was  to  move  the  robot  joint  to  a  home 
location  and  wait  for  a  response  from  the  user.  After  the  response  from  the  user  was  received, 
the  robot  joint  was  moved  to  a  position  (called  location  A)  using  position  feedback  control. 
When  the  robot  joint  reached  location  A,  the  joint  was  moved  to  location  B  using  velocity 
feedback  control.  When  location  B  was  reached,  the  joint  was  moved  back  to  location  A  using 
position  control.  The  cycling  back  and  forth  between  the  locations  A  and  B  was  performed  until 
a  signal  was  received  from  the  user.  When  the  signal  was  received,  the  robot  joint  was  moved 
back  to  the  home  location.  The  user  had  the  capacity  to  alter  the  numerical  values  of  the  three 
locations  (home,  A,  and  B)  through  the  user  interface. 
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The  data  base  structures  for  input  and  output  drivers  (presented  earlier  in  Figures  5.3 
and  5.4)  and  macros  (presented  earlier  in  Figure  5.9)  are  shown  collectively  in  Figure  5.16. 
These  were  typical  data  base  structures  that  were  provided  with  a  robot  system.  Two 
additional  macros  were  defined  for  use  by  the  programmer  to  make  specification  of  the 
feedback  control  mode  easier  to  understand  in  the  program.  The  letter  P  represented  joint 
position  feedback  and  V  represented  joint  velocity  feedback. 


Name: 
Function: 

Joint  0  input  driver. 

Obtain  the  position  and  velocity  of  joint  0. 

Name 

Field 

Type 

Range 

Log 

Comment 

po 

result 

double 

<  35.0  &&> -35.0 

1 

position  of  joint  0  (degrees) 

vO 

result 

double 

<  150,0  &£>  -150.0 

1 

velocity  of  joint  0  (degrees /second) 

Name:       Joint  0  output  driver. 

Function:    Calculate  and  output  a  control  value  to  D/A  converter  connected  to  servo-amplifier. 

Control  modes:  P  =  position  feedback,  V  i 

:  velocity  feedback. 

Name 

Field 

Type 

Range 

Log 

Comment 

vOcw 

result 

int 

<  2047  &&> -2047 

1 

joint  0  servo-valve  control  word 

jointOctI 

activation 

int 

PIV 

1 

feedback  mode 

pOsp 

set  point 

double 

<  35.0  &&> -35.0 

1 

joint  0  position  set  point  (degrees) 

vOsp 

set  point 

double 

<  150.0  &&>  -1500 

1 

joint  0  velocity  set  point  (degrees) 

Macro  Definitions. 

Name 

Replacement  String 

Comment 

TRUE 

1 

Boolean  true  value 

FALSE 

0 

Boolean  false  value 

ABS(x) 

((x)<0.0>?(-00):(x) 

Absolute  value  of  expression  x 

P 

0 

For  specifying  position  feedback  control 

V 

1 

For  specifying  velocity  feedback  control 

Figure  5.16  Data  base  structures  for  input  and  output  drivers  and  macro  definitions. 


The  program  developed  to  implement  the  task  is  shown  in  Figure  5.17.  To  begin  the 
programming,  the  state  planning  form  was  completed.  Three  states  were  required  to  accomplish 
the  defined  task.  The  start_up  state  established  joint  position  feedback  control  and 
established  the  position  set  point  to  a  home  location.  The  position_A  state  moved  the  robot 
joint  to  location  A  using  position  feedback  control.  The  position_B  state  moved  the  robot  joint  to 
location  B  using  velocity  feedback  control. 

Four  assignment  parameters  were  defined — three  to  represent  joint  location  and  one  to 
represent  how  fast  to  move  the  joint  to  location  B.  The  home  location  for  joint  0  was  defined  as 
pOhome  and  given  an  initial  value  of  0.0  degrees.  The  numerical  value  of  the  home  location 
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State  Name 

Alias  Names 

Comment 

start_up 

Using  joint  position  feedback  control,  move  joint  to  home  location.  Activate  position_A  state  if  cycling 
system  flag  evaluated  true 

position  A 

Using  joint  position  feedback  control,  move  joint  to  location  A.  If  cycling  system  flag  evaluated  false, 
activate  the  start_up  state.  When  current  position  of  joint  is  close  to  position  A,  activate  position  B  state. 

posit  ion_B 

Using  joint  velocity  feedback  control,  move  joint  to  location  B.  If  cycling  system  flag  evaluated  false, 
activate  the  start_up  state.  When  current  position  of  joint  to  close  to  position  B,  activate  position„A  state. 

Assignment  Parameters 

Name 

Assign  to 
parameter 

Range 

Initial 

Log 

Comment 

pOhome 

p*p 

<  pOloca  &&  >  pOlocb 

0.0 

c 

joint  0  home  position 

pOloca 

pOsp 

<  35.0  &&>  pOhome 

20.0 

c 

joint  0  location  A 

pOlocb 

pfcp 

<  pOhome  &4  >  -35.0 

-20.0 

c 

joint  0  location  B 

vOvelocity 

vOsp 

<  0.0  &&>  -150.0 

-5.0 

c 

velocity  to  move  to  location  B 

System  Parameters 

Name 

Type 

Range 

Initial 

Log 

Comment 

cyclejoint 

flag 

TRUE  H  FALSE 

FALSE 

start  cycling  joint  &  TRUE 

Model  Name: 
Function: 

Position  tolerance. 

Determined  when  the  current  position  of  joint  0  was  close  to  its  position  set  point, 
from  position  set  point,  otherwise  set  fpOclose  to  false. 

Set  fpOclose  to  true  if  joint  0  position  was  ±  tolerance 

Name 

Field 

Type 

Range 

Initial 

Log 

Comment 

fpOclose 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  close  to  set  point  =  TRUE 

pOtoler 

set  point 

double 

>=0.0 

SJO 

c 

error  tolerance  for  joint  0 

Logic        iff  ABS(p0  -  pOsp)  <  pOtoler )             fpOclose  =  TRUE; 

else                                              fpOclose  =  FALSE; 

Model  Name: 
Function: 

Joint  0  out-of-limits. 

Determine  when  the  position  of  joint  0  is  above  or  below  position  limits.  If  position  of  joint  0  is  below  minimum  or  above  maximum 
position  limits,  set  fpOool  to  true.  Set  fpOool  to  false  if  the  position  of  joint  0  is  within  minimum  and  maximum  position  limits. 

Name 

Field 

Type 

Range 

Initial 

Log 

Comment 

fpOool 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  0  out  of  position  limits 

pOmax 

set  point 

double 

>=  0.0  &&«  35.0 

3S.0 

1 

maximum  position  limit 

pOmin 

set  point 

double 

c=  0.0  &&>= -35.0 

-35.0 

1 

minimum  position  limit 

Logic 

if((p0>  p0max)ll(p0 
else 

<pOmin)) 

fpOool  =  TRUE; 
fpOool  -  FALSE; 

Safety  Links 

State  Link 

Link  Logic 

Comment 

start-up 

1.  fpOool 

joint  0's  position  is  out-of-limits 

A 

B 

C 

D 

0 

Initialization 

start_up 

position_A 

posit  ion_B 

1 

jointOctl 

P 

P 

V 

2 

pOsp 

pOhome 

pOloca 

pOlocb 

3 

vOsp 

vOvelocity 

4 

cycle_joint 

FALSE 

5 

pOmin 

6 

pOmax 

7 

pOtoler 

8 

State  Link 

start_up 

position_A 

posit  ion_B 

» 

start_up 

1.  Icyclejoint 

1.  Icyclejoint 

10 

position  A 

1.  cycle_joint 

2.  fpOclose 

11 

posit  ion_B 

2.  fpOclose 

Figure  5.17  State  programming  form,  assignment  and  system  parameters,  models,  and  state 
network  map  for  example  program. 
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was  restricted  in  the  Range  column  to  be  between  locations  A  and  B.  Location  A  for  joint  0  was 
defined  as  pOloca  with  an  initial  value  of  20.0  degrees  and  location  B  was  defined  as  pOlocb 
with  an  initial  value  of  -20.0  degrees.  Location  A  was  restricted  to  be  greater  than  the  home 
location  and  less  than  the  maximum  joint  position  limit  (given  in  the  data  base  structure  for  the 
joint  input  driver).  The  range  for  location  B  was  restricted  to  be  less  than  the  home  location  and 
greater  than  the  minimum  joint  position  limit.  The  velocity  assignment  parameter  (vOvelocity) 
was  defined  and  given  an  initial  value  of  -5.0  degrees  per  second.  The  range  for  the  velocity 
parameter  was  restricted  to  be  less  than  0.0  and  greater  than  the  minimum  velocity  limit. 

A  system  flag  was  needed  to  indicate  when  the  cycling  between  locations  A  and  B  was 
started.  When  the  user  wanted  the  robot  joint  to  start  cycling  between  locations  A  and  B,  the 
cyclejoint  parameter  was  set  to  true  through  the  user  interface.  The  cyclejoint  flag  was  set  to 
false  when  the  user  wanted  to  stop  the  cycling  between  locations  A  and  B  and  return  the  robot 
joint  to  the  home  location. 

A  model  to  determine  when  the  joint  was  close  to  its  position  set  point  was  programmed. 
The  model  calculated  the  difference  in  the  actual  joint  position  and  the  position  set  point.  The 
ABS(x)  macro  was  used  to  calculate  the  absolute  value  of  the  position  error.  A  comparison 
between  the  absolute  value  of  the  position  error  and  a  position  tolerance  value  (model 
parameter  pOtoler)  was  then  made.  A  true  result  indicated  that  the  joint  position  was  close  to 
its  position  set  point. 

Another  model  was  programmed  for  use  in  safety  links.  The  joint  0  out-of-limits  model 
(previously  presented  in  Figure  5.8)  set  fpOool  to  true  if  the  current  joint  position  was  below  a 
minimum  position  limit  or  above  a  maximum  position  limit.  A  safety  link  was  programmed 
using  the  result  from  the  out-of-limits  model.  The  safety  link  logic  evaluated  the  status  of  the 
fpOool  flag  and  activated  the  start_up  state  if  the  fpOool  parameter  evaluated  true. 

The  actions  and  links  for  the  three  states  were  programmed  in  a  state  network  map. 
The  rows  and  columns  of  the  state  network  map  were  labeled  to  facilitate  explanation.  Rows  1 
through  7  contained  state  initialization  statements.  Cells  1A,  2A,  and  3A  corresponded  to 
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activation  and  set  point  parameters  from  the  output  driver  data  base  structure.  Entries  in  cells 
IB,  1C,  or  ID  established  the  method  of  joint  feedback  control  as  either  position  (P  in  cell)  or 
velocity  (V  in  cell).  Cells  2A  and  3A  corresponded  to  set  point  parameters  from  the  output 
driver  data  base  structure.  Entries  in  cells  2B,  2C,  and  2D  established  the  value  of  the  position 
set  point  when  a  state  was  activated.  Likewise,  entries  in  cells  3B,  3C,  and  3D  established  the 
value  of  the  velocity  set  point  parameter.  Cells  5A,  6A,  and  7  A  were  modeling  set  point 
parameters  gathered  from  model  programming  forms.  If  the  programmer  desired  different 
modeling  criteria  when  a  state  was  activated,  modeling  set  points  were  changed  by  making  an 
entry  in  the  appropriate  intersecting  cell.  In  this  example,  no  modeling  changes  were  made  so 
the  cells  at  the  intersection  of  rows  5, 6,  and  7  and  columns  B,  C,  and  D  were  left  blank. 

State  links  were  programmed  in  rows  9  through  11.  A  blank  entry  at  the  intersection  of 
a  state  column  and  a  state  link  row  indicated  that  the  two  states  were  not  linked.  For  example, 
the  start_up  state  was  only  linked  to  the  position_A  state.  No  state  link  existed  between  the 
start_up  state  and  itself  or  to  the  position_B  state. 

An  examination  of  the  sequence  of  state  activation  demonstrated  how  the  program 
operated.  The  start_up  state  was  activated  during  system  initialization  by  the  scheduler. 
During  the  initialization  of  the  start_up  state,  three  data  base  changes  were  made.  The  value 
of  the  activation  parameter  (jointOctl)  was  set  to  0  using  the  P  macro  (cell  IB),  the  position  set 
point  for  joint  0  (pOsp)  was  changed  to  the  value  of  pOhome  (cell  2B),  and  the  cyclejoint 
parameter  was  set  to  false  (cell  4B).  At  the  beginning  of  a  control  cycle,  the  scheduler 
activated  the  input  driver.  The  joint  input  driver  obtained  and  stored  the  joint  position  and 
velocity  in  the  data  base. 

The  two  models  were  then  executed.  The  position  tolerance  model  calculated  the  error 
between  the  current  joint  position  and  the  position  set  point  and  compared  the  error  magnitude 
against  an  error  tolerance  criterion.  If  the  current  joint  position  was  close  to  the  position  set 
point,  the  model  result  (fpOclose)  was  set  to  true.  Otherwise,  the  model  result  was  set  to  false. 
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The  joint  out-of-limits  model  examined  the  current  joint  position  to  determine  if  the  joint 
position  exceeded  minimum  or  maximum  position  limits. 

The  scheduler  then  examined  safety  links.  Only  one  safety  link  was  programmed  for 
this  example  and  was  a  test  on  the  out-of-limits  model  result  fpOool.  If  the  robot  joint  was  out 
of  minimum  or  maximum  position  limits  (fpOool  evaluated  to  true),  the  start_up  state  was 
activated.  After  the  safety  links  had  been  examined,  the  state  links  were  examined.  Since  the 
start_up  state  was  active,  the  status  of  the  cyclejoint  parameter  was  evaluated  (cell  10B). 
Provided  that  the  user  had  not  altered  the  value  of  the  cyclejoint  parameter,  the  cyclejoint 
parameter  evaluated  to  false  and  a  switch  to  the  position_A  state  was  not  made.  The  start_up 
state  remained  active. 

The  joint  output  driver  was  then  activated  to  calculate  a  control  value  using  position 
feedback  control  and  send  the  value  to  a  servo-valve.  The  activation  status  of  the  data  logger 
was  then  examined  and,  if  true,  the  scheduler  signalled  activation  of  the  data  logger  program. 
The  scheduler  then  returned  to  the  timing  function  block  and  suspended  execution  until  the  next 
activation  signal  was  received.  The  next  pass  through  the  control  cycle  performed  similar 
activities. 

To  start  the  robot  joint  cycling  between  locations  A  and  B,  the  user  altered  the  value  of 
the  cyclejoint  parameter  to  true.  During  the  next  control  cycle,  the  scheduler  performed  a  state 
switch  to  the  position_A  state  because  the  state  link  in  cell  10B  evaluated  true.  The  start_up 
state  was  deactivated  and  the  position_A  state  was  initialized  by  establishing  position 
feedback  control  (cell  1C)  and  altering  the  position  set  point  to  location  A  (cell  2C)  using  the 
pOloca  assignment  parameter.  The  output  driver  used  the  new  position  set  point  to  calculate  the 
value  to  send  to  the  joint  actuator.  The  position_A  state  remained  active  until  the  cyclejoint 
parameter  was  set  to  false  by  the  user,  the  position  of  joint  0  was  close  to  the  position  set  point, 
or  a  safety  link  evaluated  true.  If  the  cyclejoint  parameter  evaluated  false,  the  start_up 
state  was  activated.  If  the  cyclejoint  parameter  evaluated  true,  the  next  state  link  was 
evaluated.  This  state  link  tested  the  model  result  fpOclose  to  determine  if  the  robot  joint  was 
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close  to  the  position  set  point.  If  joint  0  was  close  to  its  position  set  point,  the  position_B  state 
was  activated.  If  the  joint  was  not  close  to  the  position  set  point  and  the  cyclejoint  parameter 
was  true,  the  position_A  state  remained  active. 

When  the  robot  joint  was  close  to  location  A,  the  position_B  state  was  activated.  The 
position_B  state  established  velocity  feedback  control  (V  in  cell  ID),  altered  the  position  set 
point  to  location  B  (pOlocb  assignment  parameter  in  cell  2D),  and  altered  the  velocity  set  point 
to  vOvelocity  (cell  3D).  The  output  driver  now  used  velocity  feedback  to  calculate  the  control 
word.  When  the  position  of  joint  0  was  close  to  the  position  set  point,  a  switch  to  the 
position_A  state  was  made.  Notice  that  although  velocity  control  was  used  to  calculate  the 
feedback  value,  the  position  of  joint  0  caused  the  switch  to  the  next  state.  The  cycling  back  and 
forth  between  locations  A  and  B  continued  until  the  user  set  the  cyclejoint  parameter  to  false  or 
the  safety  link  evaluated  true.  If  the  cyclejoint  parameter  was  set  to  false,  the  start_up  state 
was  activated. 

Although  the  position  tolerance  set  point  was  not  altered  by  any  of  the  three  states 
during  state  initialization,  the  capacity  to  alter  the  tolerance  value  was  available  through 
the  user  interface.  If  the  user  wanted  to  test  the  robot  cycling  program  using  a  different 
tolerance  criterion  for  the  model,  the  user  altered  the  tolerance  parameter  to  a  different 
numerical  value  through  the  user  interface.  The  position  tolerance  model  immediately  started 
using  the  new  value.  If  the  user  wanted  locations  A  or  B  to  be  different  numerical  values,  the 
assignment  parameters  pOloca  and  pOlocb  were  altered  through  the  user  interface.  If  a  location 
had  been  hard  coded  in  the  state  definitions  (for  example,  20.0  had  been  programmed  in  cell  2C 
instead  of  using  the  assignment  parameter  pOloca),  on-line  changes  could  not  be  made  in  that 
location.  If  a  different  value  was  needed  for  location  A,  programmer  had  to  shut  down  the 
robot,  return  to  the  programming  environment  to  alter  the  initialization  section,  and  recompile 
the  robot  program.  Using  the  assignment  parameters  allowed  the  user  to  alter  values  of 
locations  A  and  B  on-line. 
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Through  this  sample  example,  several  features  of  the  picking  language  were 
illustrated.  The  definition  of  system  parameters  provided  communication  between  the  user 
interface  and  the  robot  program  and  gave  the  user  the  capacity  to  direct  activity.  By  creation 
of  a  model  with  on-line  adjustable  criterion,  different  operational  criterion  could  be  specified 
without  having  to  recompile  the  robot  program.  The  use  of  global  safety  links  relieved  the 
programmer  from  having  to  specify  safety  links  in  every  state  programmed.  The  assignment 
parameter  vOvelocity  was  defined  to  establish  the  speed  and  direction  of  joint  velocity  in  the 
position_B  state.  If  the  programmer  had  not  set  the  initial  value  of  this  parameter  to  the 
correct  sign,  the  robot  joint  would  move  away  from  the  position  set  point.  Thus,  the  state  link 
evaluating  when  the  joint  reached  the  position  set  point  would  never  evaluate  true.  The  safety 
link  would  determine  when  a  minimum  or  maximum  position  limit  was  exceeded  and  halt  the 
cycling  sequence  by  activation  of  the  start_up  state. 


CHAPTER  6 
PICKING  PROGRAM 


This  chapter  discusses  the  program  developed  to  control  a  vision-servoed  fruit-picking 
robot.  The  data  base  structures  defined  by  developers  of  input  and  output  driver  software  and 
program  components  (assignment  and  system  parameters,  macros,  safety  links,  models,  state 
planning  form,  and  state  network  map)  are  completely  presented  in  the  Appendices.  During 
discussion  of  the  program,  states  from  the  state  network  map,  examples  of  models,  and  partial 
listings  of  parameter  definitions  are  presented  in  the  text.  Data  base  structures  defined  for 
driver  software  are  given  in  Appendix  A  along  with  definitions  of  assignment  and  system 
parameters  and  macro  defined  by  driver  developers.  Assignment  and  system  parameter 
definitions  and  macros  programmed  for  the  picking  program  are  given  in  Appendix  B.  Safety 
links  are  presented  in  Appendix  C.  The  state  planning  form  is  given  in  Appendix  D  and  the 
state  network  map  is  given  in  Appendix  E. 

A  summary  of  the  parameters  used  by  input  and  output  drivers  is  given  to  familiarize 
the  reader  with  results  quantified  by  input  drivers  and  parameters  used  by  output  drivers.  The 
program  developed  to  control  the  robot  is  then  discussed.  The  program  was  divided  into 
different  state  sequences  for  discussion  purposes.  Power  up  and  power  down  sequences  were 
programmed  to  systematically  control  robot  motion  during  application  and  removal  of  robot 
hydraulic  power.  A  state  was  programmed  to  provide  branching  to  control  sequences  and  the 
picking  sequence.  Control  sequences  were  programmed  to  allow  developers  of  joint  output 
drivers  to  tune  position,  velocity,  and  vision  feedback  gains  on-line.  A  picking  sequence  was 
developed  to  perform  basic  picking  operations  and  was  extended  to  react  to  conditions 
encountered  during  grove  operation. 
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Parameters  Used  by  Driver  Software 


This  section  describes  parameters  used  by  input  and  output  drivers.  Software  operations 
of  drivers  were  discussed  in  chapter  4.  Data  base  structures  for  input  and  output  drivers  are 
presented  in  Appendix  A.  A  summary  of  result  parameters  calculated  by  input  driver  software 
is  shown  in  Table  6.1. 


Table  6.1  Summary  of  result  parameters  calculated  by  input  drivers. 


Name 


pO 

Pi 
P2 
vO 
vl 
v2 
jpO 

jpl 
JP2 
distp2 
p2pick 
psdmp 
pssapr 
pspbut 
psjbutO 
psjbutl 
fblob 
xcent 
xdia 
ycent 
ydia 
iavef 
iaveb 


Comment 


position  of  joint  0  (degrees) 

position  of  joint  1  (degrees) 

position  of  joint  2  (centimeters) 

velocity  of  joint  0  (degrees/ second) 

velocity  of  joint  0  (degrees/second) 

velocity  of  joint  2  (centimeters/second) 

joy  stick  offset  position  for  joint  0  (A/D  units) 

joy  stick  offset  position  for  joint  1  (A/D  units) 

joy  stick  offset  position  for  joint  2  (A/D  units) 

distance  from  end-effector  to  object  (centimeters) 

position  of  joint  2  for  object  removal  (centimeters) 

power  status  of  HPU  dump  valve 

power  status  of  servo-amplifier 

power  status  of  console  push  buttons 

power  status  of  joy  stick  push  button  #0 

power  status  of  joy  stick  push  button  #1 

set  to  true  if  a  valid  object  located  in  image 

object  horizontal  centroid  location  in  image  (pixels) 

horizontal  diameter  of  object  (pixels) 

object  vertical  centroid  location  in  image  (pixels) 

vertical  diameter  of  object  (pixels) 

average  intensity  of  fruit  pixels 

average  intensity  of  background  pixels 


The  position  (pO,  pi,  and  p2)  and  velocity  (vO,  vl,  and  v2)  of  each  robot  joint  were 
quantified  by  joint  input  drivers  and  results  stored  in  the  data  base.  Joy  stick  position  offsets 
were  quantified  and  stored  in  the  data  base  under  the  parameter  names  jpO,  jpl,  and  jp2.  The 
distance  from  the  ultrasonic  transducer  to  an  object  located  in  front  of  the  end-effector  was 
stored  in  the  distp2  result  parameter  and  a  position  for  object  removal  was  calculated  for  joint  2 
(p2pick).  The  power  status  of  the  HPU  dump  valve  (psdmp),  the  servo-amplifier  (pssapr),  the 
console  push  buttons  (pspbut),  and  the  joy  stick  push  buttons  (psjbutO  and  psjbutl)  were  sensed. 
Information  about  an  object  located  in  an  image  by  a  vision  driver  (fblob,  xcent,  xdia,  ycent,  and 
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ydia)  were  calculated  and  stored  in  the  data  base.  A  true  fblob  result  indicated  that  an  object 
that  met  minimum  size  criteria  had  been  located  in  the  image.  The  horizontal  and  vertical 
centroids  of  an  object  located  in  an  image  were  stored  in  xcent  and  ycent  parameters.  The 
horizontal  and  vertical  diameter  measurements  of  a  valid  object  were  stored  in  the  xdia  and 
ydia  parameters.  The  average  intensity  of  pixels  examined  during  object  quantification  was 
stored  in  the  iavef  result  parameter  if  a  valid  object  was  located  in  the  image.  The  average 
intensity  of  pixels  examined  during  object  location  was  stored  in  the  iaveb  result  parameter  if  a 
valid  object  could  not  be  located  in  the  image. 

A  summary  of  parameters  used  by  output  driver  software  is  shown  in  Table  6.2.  Output 
drivers  for  joints  0, 1,  and  2  used  an  activation  parameter  to  indicate  which  feedback  mode  was 
used  to  calculate  a  joint  command.  Four  macros  were  defined  to  establish  joint  feedback  mode 
(see  Appendix  A  for  a  listing  of  macros  for  drivers).  The  P,  V,  and  J  macros  were  defined  to 
specify  position,  velocity,  or  joy  stick  feedback  control  for  joints  0, 1,  or  2.  The  VI  macro  was 
used  to  specify  vision  feedback  control  for  joints  0  and  1.  Position  (pOsp,  plsp,  and  p2sp), 
velocity  (vOsp,  vlsp,  and  v2sp),  and  vision  (viOsp  and  vilsp)  set  point  parameters  were  used  by 
joint  output  drivers  to  calculate  servo-valve  control  values  (vOcw,  view,  and  v2cw). 

The  output  driver  for  the  aperture  motor  used  average  intensity  results  from  the  vision 
input  driver  to  calculate  an  aperture  command  (aptcw).  This  feedback  was  automatic  and  a  set 
point  was  not  available  to  the  picking  language.  Three  activation  parameters  were  used  to 
indicate  the  status  of  open-loop  actuation  devices.  The  fhydpwr  activation  parameter  was 
used  to  specify  the  power  status  to  the  external  push  button.  The  end-effector  lighting  system 
activation  parameter  (flights)  was  used  to  specify  the  status  of  power  to  the  end-effector 
lights.  The  ON  and  OFF  macros  were  used  to  alter  the  value  of  fhydpwr  and  flights.  The  ON 
macro  was  used  to  specify  power  to  a  device  and  the  OFF  macro  was  used  to  remove  power  from 
a  device.  The  picking  device  activation  parameter  (pdevice)  was  used  to  specify  the  status  of 
power  to  the  picking  device  solenoids.  Macros  defined  for  setting  the  status  of  the  picking 
device  were  OFF,  OPEN,  and  CLOSED.  The  OFF  macro  specified  that  both  picking  device 
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Table  6.2  Summary  of  parameters  used  by  output  drivers. 


neiQ 

rName 

v^iTirneni 

result 

vOcw 

servo-valve  control  word  for  joint  0  (D/A  units) 

activation 

jointOctl 

specification  of  type  of  closed-loop  feedback  for  joint  0 

set  point 

pOsp 

position  set  point  for  joint  0 

set  point 

vOsp 

velocity  set  point  for  joint  0 

set  point 

viOsp 

vision  set  point  for  joint  0 

result 

view 

servo-valve  control  word  for  joint  1  (D/A  units) 

activation 

jointlctl 

specification  of  type  of  closed-loop  feedback  for  joint  1 

set  point 

plsp 

position  set  point  for  joint  1 

set  point 

vlsp 

velocity  set  point  for  joint  1 

set  point 

vilsp 

vision  set  point  for  joint  1 

result 

v2cw 

servo-valve  control  word  for  joint  2  (D/A  units) 

activation 

joint2ctl 

specification  of  type  of  closed-loop  feedback  for  joint  2 

set  point 

p2sp 

position  set  point  for  joint  2 

set  point 

v2sp 

velocity  set  point  for  joint  2 

result 

aptcw 

control  word  for  lens  aperture  opening 

set  point 

fhydpwr 

power  status  to  external  push  button 

set  point 

flights 

end-effector  lights  power  status 

set  point 

pdevice 

picking  device  actuator  power  status 

solenoids  were  deactivated.  The  OPEN  macro  specified  that  the  picking  device  should  be 
moved  to  the  open  location  and  the  CLOSED  macro  specified  that  the  picking  device  should  be 
moved  to  the  closed  location. 

A  compiler  for  the  picking  language  would  collect  activation,  set  point,  and  system 
parameters  from  data  base  structures  and  list  these  parameters  in  the  initialization  section  of  a 
state  network  map.  The  14  activation  and  set  point  parameters  defined  in  output  driver  data 
base  structures  are  shown  in  column  A  in  Figure  6.1.  The  state  activated  at  system  initialization 
(start_up)  is  shown  ready  to  program  in  column  B.  Developers  of  the  data  logger  system 
defined  a  system  parameter  (fdata)  to  indicate  the  status  of  logging  activity.  Although 
normally  set  through  the  user  interface,  a  state  specified  logging  activity  by  changing  the 
value  of  the  fdata  parameter.  The  ON  and  OFF  macros  were  used  to  alter  the  status  of  the 
data  logger. 

Power  Up  and  Power  Down  Sequences 
State  sequences  were  programmed  to  systematically  apply  and  remove  robot  hydraulic 
power.  A  power  up  sequence  was  programmed  to  apply  hydraulic  power  to  the  robot  and 
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Initialization 

start_up 

jointOctl 

jointlctl 

joint2ctl 

P*P 

plsp 

p2sp 

vOsp 

vlsp 

v2sp 

viOsp 

vilsp 

fhydpwr 

flights 

pdevice 

fdata 

State  Links 

start  up 

start_up 

Figure  6.1  Collection  of  activation,  set  point,  and  system  parameters  from  data  base  structures. 

systematically  move  robot  joints  to  known  locations.  A  power  down  sequence  was  programmed  to 
remove  hydraulic  power  from  the  robot.  The  hydraulic  accumulator  (refer  to  Figure  4.3  in 
chapter  4)  stored  hydraulic  energy  for  use  when  joint  actuators  demanded  a  higher  flow  rate 
than  the  hydraulic  pump  could  supply.  When  hydraulic  power  was  applied  to  the  robot,  the 
hydraulic  pump  supplied  joint  actuators  and  charged  the  accumulator.  When  hydraulic  power 
was  removed  from  the  robot,  hydraulic  power  was  available  to  joint  actuators  for  a  limited 
time  while  the  accumulator  discharged.  The  power  down  sequence  utilized  the  accumulator 
energy  to  move  robot  joints  to  known  locations. 

The  state  planning  form  and  the  state  network  map  for  the  power  up  and  power  down 
sequences  (extracted  from  Appendices  E  and  F)  are  shown  in  Figure  6.2.  The  rows  and  columns  of 
the  state  network  map  are  labeled  so  individual  cells  can  be  referenced.  Columns  B  through  F 
contain  the  initialization  and  links  for  the  five  states  programmed  for  the  power  up  and  power 
down  sequences.  The  data  base  parameters  that  were  altered  during  power  up  and  power  down 
were  listed  in  column  A  in  rows  2  through  24.  Entries  in  rows  2  through  16  were  the  parameters 
used  by  output  drivers  and  the  data  logger  system  parameter.  Rows  17  through  24  are  system 
and  modeling  set  point  parameters  defined  for  the  power  up  and  power  down  sequences.  The 
state  links  programmed  for  the  power  up  and  power  down  sequences  are  shown  in  rows  26 
through  30.  A  state  (name  listed  at  the  top  of  each  column)  was  linked  to  another  state  (name 
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listed  in  column  A  in  the  state  link  section)  if  an  expression  was  programmed  at  the  intersection 
of  the  column  and  row. 


State  Name 

Alias  Names 

Comment 

dump_go_mid 

Power  down  robot.  Using  position  feedback  control,  move  joints  0,1,  and  2  to  mid  locations.  When  all  the  positions 
of  the  three  joints  are  close  to  the  position  set  points  and  velocities  of  joints  are  low,  activate  the  start_up  state. 

dump_pull_in 

Power  down  robot.  Using  position  feedback  control,  move  joint  2  to  mid  location  keeping  joints  0  and  1  at  current 
locations.  When  velocity  of  joint  2  is  low  and  position  is  close  to  mid  location,  activate  dump  go  mid  state. 

go  mid 

Using  position  feedback  control,  move  joints  0, 1,  and  2  to  mid  locations. 

pull_in 

Using  position  feedback  control,  move  joint  2  to  mid  location  keeping  joints  0  and  1  at  current  locations.  When 
joint  2  is  close  to  mid  location  and  moving  slowly,  activate  go  mid  state. 

start_up 

State  activated  when  system  initialized.  Make  sure  power  is  removed  from  robot.  Establish  position  control  on 
joints  0, 1,  and  2  using  current  positions  for  position  set  points.  Activate  pull_in  state  when  hydraulic  power  flag  is 
set  to  true. 

A 

B 

C 

D 

E 

F 

1 

Initialization 

dump_go_mid 

dump_pull_in 

go_mid 

pulljn 

start_up 

2 

jointOctl 

P 

P 

P 

P 

P 

3 

jointlctl 

P 

P 

P 

P 

P 

4 

joint2ctl 

P 

P 

P 

P 

P 

5 

pCsp 

pOmid 

po 

pOmid 

fO 

P° 

6 

plsp 

pi  mid 

Pi 

pi  mid 

Pi 

Pi 

7 

p2sp 

p2mid 

p2mid 

p2mid 

p2mid 

P2 

8 

vOsp 

9 

vlsp 

10 

v2sp 

11 

viOsp 

12 

vilsp 

13 

fhydpwr 

OFF 

OFF 

ON 

ON 

OFF 

14 

flights 

OFF 

OFF 

OFF 

OFF 

OFF 

15 

pdevice 

OFF 

OFF 

OFF 

OFF 

OFF 

16 

fdata 

ON 

17 

fpowerup 

OFF 

OFF 

OFF 

18 

drtime 

180 

240 

120 

19 

pOslop 

It 

20 

pi  slop 

u 

21 

p2slop 

4.4 

4.4 

44 

22 

vOslop 

14 

23 

vlslop 

U 

24 

v2slop 

0.7 

0.7 

07 

23 

State  links 

dumpgo  mid 

dump_pull_in 

go_mid 

putl_in 

start  up 

26 

dump_go_mid 

1.  fv2slop  &&  (fp2slop  II  fdt) 

27 

dump_pull_in 

1.  tfpowerup 

28 

go  mid 

1.  fV2slop  SlA  (fp2slop  II  fdt) 

29 

pulMn 

1.  fpowerup 

30 

start_up 

l.fvslop  SlSl  (fpslop  II  fdt) 

Figure  6.2  State  planning  form  and  state  network  map  for  power  up  and  power  down  sequences. 


Figure  6.3  presents  a  sample  of  the  models  and  definition  of  assignment  and  system 
parameters  used  during  the  power  up  and  power  down  sequences.  Definition  of  assignment  and 
system  parameters  can  be  found  in  Appendices  A  and  B.  A  complete  display  of  all  the  models 
programmed  can  be  found  in  Appendix  D. 

The  timer  model  determined  when  a  state  had  been  active  for  a  specified  time  interval. 
The  model  criterion  dttime  specified  the  number  of  control  cycles  a  state  was  active  before  the 
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Model  Name: 
Function: 

Timer 

Determined  when  duration  of  state  exceeded  a  specified  time  limit.  Set  fdt  to  true  if  state  active  time  (state  .time)  was  greater  than 
desired  state  active  time  (dttime),  otherwise  set  fdt  to  false. 

Name 

Field 

Type 

Range 

Initial 

Log 

Comment 

fat 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

result  flag 

dttime 

set  point 

long  int 

5=0 

0 

state  active  time  0  /60)  second 

Logic         ifi  dttime  <  statejime)          fdt  =  TRUE; 

else                                   fdt  =  FALSE; 

Model  Name: 
Function: 

Joint  0  position  error 

Determined  when  joint  0's  position  error  magnitude  was  less  than  allowable  limit.  Set  fpOslop  to  true  if  position  of  joint  0  (pO)  was 
between  ±  position  tolerance  (pOslop)  of  position  set  point  (pOsp),  otherwise  set  fpOslop  to  false. 

Name 

Field 

Type 

Range 

Initial 

L°g 

Comment 

fpOslop 

resu  It 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  0  position  slop  flag 

pOslop 

set  point 

double 

>0.0 

16 

c 

joint  0  allowable  error 

Logic         if(  ABS(pO  -  pOsp)  <  pOslop)     fpOslop  =  TRUE; 

else                                   fpOslop  =  FALSE; 

Model  Name: 
Function: 

Joint  0  velocity 

Determined  when  joint  0's  velocity  magnitude  was  less  than  allowable  limit.  Set  fvOslop  to  true  if  velocity  of  joint  0  (vO)  was  between  + 
velocity  tolerance  (vOslop)  of  velocity  set  point  (vOsp),  otherwise  set  fvOslop  to  false. 

Name 

Field 

Type 

Range 

Initial 

Log 

Comment 

fvOslop 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  0  velocity  slop  flag 

vOslop 

set  point 

double 

>0.0 

48.8 

c 

joint  0  velocity  slop  limit 

Logic         if(  ABS<  vO )  <  vOslop  )           fvOslop  =  TRUE; 

else                                   fvOslop  =  FALSE; 

Assignment  Parameters 

Name 

Assign  to 
Parameter 

Range 

Initial 

Log 

Comment 

pOmid 

pOsp 

0.0 

c 

middle  position  for  joint  0  (deg) 

pi  mid 

P!«P 

00 

c 

middle  position  for  joint  1  (deg) 

p2mid 

p2sp 

c 

middle  position  for  joint  2  (cm) 

System  Parameters 

Name 

Type 

Range 

Initial 

Log 

Comment 

fpowerup 

flag 

ON  II  OFF 

OFF 

hydraulic  power  request  flag 

Figure  6.3  Model  examples  and  assignment  and  system  parameters  defined  for  power  up  and 
power  down  sequences. 


result  (fdt)  was  set  to  true.  This  model  made  use  of  the  state_time  system  parameter  that  the 
scheduler  set  to  zero  when  a  state  was  initialized  and  incremented  at  the  beginning  of  a  control 
cycle.  The  units  for  the  dttime  set  point  parameter  were  1  /60  second  corresponding  to  the  60  Hz 
activation  rate  of  the  scheduler. 

To  detect  when  the  position  of  a  joint  was  close  to  its  position  set  point,  a  joint  position 
error  model  was  programmed.  The  joint  position  error  model  shown  in  Figure  6.3  determined 
when  joint  0's  position  was  close  to  its  position  set  point.  The  model  calculated  an  error  between 
the  current  joint  position  and  the  position  set  point  and  tested  the  error  magnitude  against  a 
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position  tolerance  criterion.  The  model  criterion  pOslop  specified  the  allowable  difference 
between  the  current  position  and  the  position  set  point.  The  model  result  (fpOslop)  was  set  to 
true  if  the  joint  position  was  close  to  its  position  set  point.  The  model  actually  programmed 
(Figure  D.2  in  Appendix  D)  determined  this  information  for  each  joint  and  set  an  additional 
result  (fpslop)  to  true  if  all  three  joints  were  close  to  their  respective  position  set  point. 

To  detect  when  a  joint  was  moving  slowly,  a  joint  velocity  model  was  programmed.  The 
velocity  model  shown  in  Figure  6.3  determined  when  the  magnitude  of  joint  O's  velocity  was  less 
than  a  tolerance  criterion.  The  model  criterion  vOslop  was  used  to  specify  how  close  the 
magnitude  of  joint  O's  velocity  had  to  come  to  zero  velocity  before  the  model  result  (fvOslop) 
was  set  to  true.  The  model  actually  programmed  (Figure  D.3  in  Appendix  D)  performed  the 
same  function  for  each  joint  and  set  an  additional  result  (fvslop)  if  all  three  joints  were  within 
their  respective  velocity  tolerance  criterion. 

An  assignment  parameter  was  defined  to  represent  a  mid  location  for  each  joint  (pOmid, 
pi  mid,  and  p2mid).  Since  the  range  cells  were  left  blank  by  developers  of  output  drivers,  the 
values  of  joint  mid  locations  could  not  be  altered  by  a  state  or  through  the  user  interface.  A 
system  parameter  (fpowerup)  was  defined  so  the  user  could  communicate  the  desired  status  of 
robot  power  to  the  program. 

The  fpowerup  system  parameter  was  included  in  the  state  network  map  in  row  17. 
Modeling  set  point  parameters  were  included  in  the  state  network  map  in  rows  18  through  24. 
Row  18  was  used  to  specify  alterations  to  the  dttime  parameter.  Rows  19, 20,  and  21  were  used 
to  alter  position  error  modeling  criteria  and  rows  22, 23,  and  24  were  used  to  alter  velocity 
modeling  criteria. 

Power  Up  Sequence 

The  power  up  sequence  consisted  of  the  start_up  (column  F),  the  pull_in  (column  E),  and 
the  go_mid  (column  D)  states.  The  power  up  sequence  started  with  power  removed  from  the 
robot.  After  the  user  signalled  the  desire  to  apply  power  to  the  robot,  the  HPU  dump  valve 
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was  energized  to  route  hydraulic  fluid  to  the  robot.  Joint  2  was  moved  to  a  mid  location  before 
implementing  horizontal  or  vertical  motion.  This  was  performed  to  prevent  the  end-effector 
from  colliding  with  tree  structures  if  the  robot  was  powered  up  when  the  end-effector  was 
inside  the  canopy.  After  joint  2  reached  its  mid  location  and  was  moving  slowly,  joints  0  and  1 
were  moved  to  their  mid  location. 

Joint  position  control  was  used  during  each  state  in  the  power  up  sequence.  Joint  position 
control  was  programmed  by  using  the  P  macro  in  cells  D2,  D3,  D4,  E2,  E3,  E4,  F2,  F3,  and  F4. 
Position  set  points  were  established  for  joints  0, 1,  and  2  according  to  the  desired  action  of  the 
state.  Velocity  and  vision  set  points  were  not  altered  during  the  power  up  sequence  and  these 
cells  were  left  blank  in  the  state  network  map. 

The  first  state  in  the  power  up  sequence  was  the  start_up  state  (the  state  activated  at 
system  initialization).  The  function  of  the  start_up  state  was  to  remove  all  robot  component 
power.  Position  set  points  were  established  for  joints  0, 1,  and  2  at  the  current  location  of  each 
joint.  This  was  programmed  by  entering  pO,  pi ,  and  p2  in  cells  5F,  6F,  and  7F.  Power  to  the 
external  push  button,  the  end-effector  lighting  system,  and  the  picking  device  were  removed  by 
using  the  OFF  macro  in  cells  13F,  14F,  and  15F.  The  activation  status  of  the  data  logger  was  not 
altered  by  leaving  cell  16F  blank. 

A  link  from  the  start_up  state  to  the  pull_in  state  was  programmed  in  cell  29F  to 
evaluate  the  status  of  the  fpowerup  system  parameter.  During  initialization  of  the  start_up 
state,  the  fpowerup  system  parameter  was  set  to  OFF  (cell  17F)  to  ensure  that  the  start_up 
state  remained  active  until  fpowerup  was  set  to  ON  through  the  user  interface.  The  pull_in 
state  was  activated  when  fpowerup  evaluated  true. 

The  pull_in  state  (column  E)  activated  robot  hydraulic  power  and  moved  joint  2  to  its 
mid  location.  The  position  set  points  for  joints  0  and  1  were  established  at  current  joint  positions 
by  using  the  result  parameters  pO  and  pi  in  cells  5E  and  6E.  The  position  set  point  for  joint  2  was 
established  at  the  mid  position  by  programming  the  p2mid  assignment  parameter  in  cell  7E. 
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Power  was  established  to  the  external  push  button  and  the  data  logger  was  activated  by  using 
the  ON  macro  in  cells  13E  and  16E. 

Before  the  pull_in  state  was  deactivated,  it  was  required  that  joint  2  was  moving 
slowly  and  (if  possible)  close  to  its  position  set  point.  Since  modeling  criteria  were  permanently 
altered  through  the  user  interface,  the  position  error  criterion  for  joint  2  (p2slop)  was  set  to  4.4 
cm  in  cell  21E  and  the  velocity  criterion  for  joint  2  (v2slop)  was  set  to  0.7  cm/s  (cell  24E)  during 
initialization.  This  ensured  that  these  criteria  were  used  by  the  models  used  while  the  pull_in 
state  was  active. 

The  link  from  the  pull_in  state  to  the  go_mid  state  evaluated  results  of  the  position 
error  and  velocity  models  to  determine  when  joint  2  was  close  to  its  position  set  point  and  was 
moving  slowly.  Because  joint  2  sometimes  failed  to  reach  its  position  set  point  (during  early 
robot  design  and  prior  to  proper  tuning  of  lag-lead  feedback  gains),  a  time  interval  test  was 
included  with  the  position  requirement.  Thus,  if  joint  2  was  moving  slowly  and  either  the  joint 
position  was  close  to  its  position  set  point  or  the  pull_in  state  had  been  active  for  two  seconds 
(dttime  set  to  120  in  cell  18E),  the  go_mid  state  was  activated. 

The  go_mid  state  (column  D)  was  similar  to  the  pull_in  state  except  that  all  three  joint 
position  set  points  were  set  to  their  respective  mid  location.  The  link  from  the  go_mid  state  to 
the  dump_pull_in  state  (cell  27D)  tested  the  status  of  the  fpowerup  parameter  to  determine  if 
the  user  wanted  to  remove  power  from  the  robot.  If  fpowerup  evaluated  false  (the  state  link 
!  fpowerup  evaluated  true),  the  go_mid  state  was  deactivated  and  the  dump_pull_in  state  was 
activated. 

Power  Down  Sequence 

The  power  down  sequence  was  developed  to  remove  hydraulic  power  from  the  robot  and 
use  the  hydraulic  energy  stored  in  the  accumulator  to  move  robot  joints  to  their  mid  location. 
After  the  HPU  dump  valve  was  deactivated,  joint  2  was  moved  to  its  mid  location  before 
implementing  horizontal  or  vertical  motion.  This  was  performed  to  prevent  the  end-effector 
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from  colliding  with  tree  structures  if  the  end-effector  was  inside  the  canopy  when  the  robot  was 
powered  down.  Joints  0  and  1  were  moved  to  their  mid  location  after  joint  2  reached  its  mid 
location.  After  all  joints  were  centrally  located,  control  was  returned  to  the  beginning  of  the 
power  up  sequence. 

The  dump_pull_in  state  was  the  start  of  the  power  down  sequence.  The  initialization 
of  the  dump_pull_in  state  (column  C  in  Figure  6.2)  was  similar  to  the  initialization  of  the 
pull_in  state  except  that  fpowerup  and  fhydpwr  were  set  to  OFF.  The  timer  model  criterion 
(dttime)  was  increased  to  four  seconds  (240  in  cell  18C)  to  allow  additional  time  for  the  end- 
effector  to  be  withdrawn  from  the  canopy.  Control  was  passed  to  the  dump_go_mid  state  when 
joint  2  was  moving  slowly  and  either  the  position  of  joint  2  was  close  to  the  position  set  point  or 
a  time  interval  had  elapsed.  A  time  interval  test  was  needed  so  this  state  did  not  remain 
active  if  joint  2  did  not  reach  its  position  set  point.  The  limited  amount  of  hydraulic  power 
stored  in  the  accumulator  and  the  distance  joint  2  had  to  travel  to  reach  its  mid  location  were 
factors  in  joint  2  failing  to  reach  its  position  set  point. 

The  dump_go_mid  state  (column  B  in  Figure  6.2)  moved  all  three  joints  to  their  mid 
location.  The  position  set  point  for  each  joint  was  established  using  assignment  parameters 
representing  joint  mid  locations.  Position  error  and  velocity  model  criteria  were  altered  during 
initialization  to  established  appropriate  values  for  joint  tolerances  during  power  down.  When 
the  velocity  of  each  joint  was  below  model  criterion  and  either  the  position  set  point  had  been 
reached  by  all  joints  or  the  dump_go_mid  state  had  been  active  for  three  seconds,  the  start_up 
state  was  activated. 

Safety  Links 

Safety  links  were  responsible  for  monitoring  system  integrity.  The  safety  links 
developed  for  this  program  and  assignment  parameters  defined  for  use  in  the  safety  links  are 
shown  in  Figure  6.4.  Fault  conditions  that  were  detected  included  movement  of  a  robot  joint 
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beyond  preset  work  space  limits  and  power  failure  to  certain  devices.  Every  fault  detected  in 
the  safety  links  resulted  in  the  activation  of  the  dump_pull_in  state. 


Assignment  Parameters 

Name 

Assign  to 
Parameter 

Range 

Initial 

Log 

Comment 

pOhi 

pOsp 

36.2 

c 

joint  0  high  position  (deg) 

pOlo 

pftp 

-36.2 

c 

joint  0  low  position  (deg) 

plhi 

pOsp 

192 

c 

joint  1  high  position  (deg) 

pllo 

plsp 

-19.2 

c 

joint  1  low  position  (deg) 

p2hi 

p2sp 

137.7 

c 

joint  2  high  position  (cm) 

p21o 

p2sp 

2.0 

c 

joint  2  low  position  (cm) 

Safety  Links 

Link  State 

Link  Logic 

Comment 

dump_pull_in 

1.  Ipsdmp  &&  fhydpwr 

no  power  sensed  to  HPU  dump  valve  and  power  supplied  to  external  push  button  (exterior  push 
button  is  open). 

dump_pull_in 

2.  fhydpwr  &&  Ifpowerup 

power  sensed  to  HPU  dump  valve  and  power  supplied  to  external  push  button  and  user  requesting 
robot  power  to  be  removed 

dump_pulMn 

3.  ipssapr 

no  power  sensed  to  servo-amplifier 

dump_pull_in 

4.  ipspbut 

no  power  sensed  to  console  push  buttons 

dump_pull_in 

5.  IpsjbutO  II  Ipsjbutl 

a  joy  stick  push  button  is  pressed 

dump_pull_in 

6.  (p0>p0hi)  H  (p0<p01o) 

joint  O's  position  is  above  or  below  position  limits 

dump_pull_in 

7.  (pi  >plhi)  It  (pi  <  pllo) 

joint  l's  position  is  above  or  below  position  limits 

dump_pulMn 

8.  (p2>p2hi)  II  (P2<p21o) 

joint  2's  position  is  above  or  below  position  limits 

Figure  6.4  Assignment  parameters  and  safety  links  programmed  for  power  up  and  power  down 
sequences. 


The  first  safety  link  determined  when  the  external  push  button  was  open.  Since  the 
status  of  the  external  push  button  was  not  sensed  directly,  a  combination  of  the  HPU  dump 
valve  not  receiving  power  (Ipsdmp  evaluated  true)  and  power  being  supplied  to  the  external 
push  button  (fhydpwr  evaluated  true)  was  used  to  indicate  that  the  external  push  button  was 
open.  The  second  safety  link  evaluated  when  power  was  applied  to  the  external  push  button 
and  the  user  requested  that  power  was  removed  from  the  robot. 

The  third  safety  link  tested  the  status  of  power  to  the  servo-amplifier.  A  fault 
condition  was  indicated  if  the  servo-amplifier  was  not  receiving  power.  The  fourth  safety  link 
tested  the  status  of  power  to  the  console  push  buttons.  The  user  opened  a  console  push  button  to 
indicate  that  the  robot  should  be  powered  down.  The  fifth  safety  link  tested  the  status  of  joy 
stick  push  buttons.  If  joy  stick  push  buttons  #0  or  #1  were  pressed,  the  robot  was  powered  down. 
The  next  three  safety  links  tested  the  position  of  joints  0, 1,  and  2  to  determine  if  a  joint  had 
moved  beyond  high  or  low  position  limits.  The  high  and  low  position  limits  were  defined  as 
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assignment  parameters  that  could  not  be  altered  through  the  user  interface  or  by  the  robot 
program  (range  cells  were  left  blank).  The  robot  was  powered  down  if  any  joint  was  beyond  one 
of  these  established  position  limits. 

Control  Sequences 

A  command  state  was  developed  to  provide  the  capacity  to  branch  to  different  state 
sequences  after  the  robot  had  been  powered  up.  System  parameters  were  used  to  specify  which 
state  sequence  was  performed.  Developers  of  joint  output  driver  software  required  sequences 
where  final  tuning  of  compensator  gains  were  accomplished  on-line.  The  states  programmed  for 
control  sequences  were  extracted  from  the  state  planning  form  (Appendix  E)  and  the  state 
network  map  (Appendix  F)  and  are  shown  in  Figure  6.5.  The  go_mid  state  (from  the  power  up 
sequence)  is  shown  to  illustrate  changes  made  in  the  definition  of  this  state.  The  other  states  in 
the  power  up  and  power  down  sequences  are  not  shown  in  this  figure  because  of  space 
considerations.  The  changes  made  in  the  go_mid  state  are  highlighted  with  a  bar  above  and 
below  the  characters  in  a  changed  cell.  Changes  to  the  go_mid  state  included  specifying 
position  error  and  velocity  modeling  criteria  and  adding  a  link  to  the  command  state.  The  link 
from  the  go_mid  state  to  the  dump_pull_in  state  was  removed  since  a  safety  link  now 
performed  this  test. 

The  model  and  assignment  and  system  parameters  defined  for  control  sequences  are 
shown  in  Figure  6.6.  When  vision  control  was  used  on  joints  0  and  1,  there  was  a  chance  that 
alignment  on  an  object  moved  either  joint  0  or  1  to  a  position  limit.  If  this  occurred,  safety  links 
indicated  that  hydraulic  power  was  removed  from  the  robot.  To  avoid  this  situation,  the 
approach  of  position  limits  had  to  be  detected.  The  out-of-limits  model  was  defined  to  detect 
when  a  joint  was  approaching  joint  limits.  The  joint  0  out-of-limits  model  shown  in  Figure  6.6 
tested  the  current  position  of  joint  0  against  minimum  and  maximum  joint  position  limits.  The 
range  cells  for  the  minimum  and  maximum  position  set  point  parameters  were  left  blank  so  the 
set  point  values  were  not  altered  by  a  state  or  through  the  user  interface.  Three  result 
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State  Name 

Alias  Names 

Comment 

command 

Using  position  feedback  control,  move  all  three  joints  to  home  locations.  Test  status  of  system  parameters  to 
determine  which  control  sequence  to  activate. 

go_home 

Move  joints  0, 1,  and  2  to  home  locations  using  position  control  When  all  three  joints  are  close  to  position  set 
points  and  moving  slowly,  activate  command  state. 

home_pull_in 

Using  position  control,  move  joint  2  to  home  location  and  keep  joints  0  and  1  at  current  locations.  When  joint  2  is 
moving  slowly  and  either  position  set  point  has  been  reached  or  two  seconds  elapse,  activate  go  home  state. 

joy_stick 

Activate  joy  stick  feedback  on  joints  0, 1,  and  2.  Activate  home_pull_in  state  if  joy  stick  flag  is  set  to  false. 

track 

Establish  vision  control  on  joints  0  and  1  to  align  end-effector  with  object  located  by  vision  input  driver.  Use 
position  control  on  joint  2  to  maintain  joint  position  at  current  location. 

Initialization 

command 

go_home 

go_mid 

home_pulMn 

joystick 

track 

jointOctl 

P 

P 

P 

P 

J 

VI 

jointlctl 

P 

P 

P 

P 

J 

VI 

joint  2  ct  1 

P 

P 

P 

P 

J 

P 

pOsp 

pOhome 

pOhome 

pOmid 

P° 

plsp 

plhome 

plhome 

pi  mid 

Pi 

p2sp 

p2home 

p2home 

p2mid 

p2home 

P2 

vOsp 

0 

vlsp 


0 

v2sp 

0 

viOmid 

vilsp 

vilmid 

fhydpwr 

ON 

ON 

ON 

ON 

ON 

ON 

flights 

OFF 

OFF 

OFF 

OFF 

OFF 

ON 

pdevice 

OPEN 

OPEN 

OFF 

OPEN 

OPEN 

OPEN 

dttime 

120 

fjoy 

FALSE 

ft  rack 

FALSE 

pOslop 

It 

16 

pi  slop 

1.8 

1.8 

p2slop 

44 

4.4 

44 

vOslop 

1.6 

1.6 

vlslop 

1.6 

1.6 

v2slop 

07 

0.7 

07 

State  Links 

command 

go_home 

go_mid 

home_pull_ln 

joy_sttck 

track 

command 

1 .  fpslop  &&  fvslop 

1.  fpslap  &&  fvslup 

go  home 

l.lv2slop&&  (fp2slop  II  fdt) 

home_pull_in 

l.lfjoy 

1.  Iftrack  II  fpool  II  !fblob 

joy_stick 

2.  fjoy 

track 

l.ftrack 

Figure  6.5  State  planning  form  and  state  network  map  for  control  sequences. 


parameters  were  set  to  indicate  joint  position  information.  If  the  joint  position  was  greater  than 
the  maximum  position,  a  maximum  out-of-limits  result  was  set  to  true.  If  joint  position  was  less 
than  the  minimum  position,  a  minimum  out-of-limits  result  was  set  to  true.  If  either  the 
minimum  or  maximum  results  were  true,  a  joint  overall  result  was  set  to  true  to  indicate  that  the 
position  of  the  joint  was  outside  of  the  minimum-maximum  range.  The  actual  model 
programmed  (Figure  D.5  in  Appendix  D)  performed  these  calculations  for  all  three  joints  and 
set  an  additional  result  to  true  if  any  joint  was  outside  of  its  minimum-maximum  range. 

An  assignment  parameter  was  defined  to  represent  a  home  location  for  each  joint.  Two 
system  parameters  were  defined  to  indicate  the  control  sequence  to  activate.  The  ftrack  system 
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Model  Name: 
Function: 

Joint  0  out-of-limits. 

Determined  if  position  of  joint  0  (pO)  was  outside  maximum  or  minimum  position  limits.  Set  fpOmax  to  true  if  position  of  joint  0  was 
greater  than  joint's  maximum  position  (pOmax),  otherwise  set  to  false.  Set  fpOmin  to  true  if  position  of  joint  0  was  less  than  joint's 
minimum  position  (pOmin).  otherwise,  set  to  false.  Set  fpOool  to  true  if  joint  position  was  outside  of  minimum  or  maximum  position 
limits.  Set  fpOool  to  false  if  joint  position  was  within  both  maximum  and  minimum  position  limits. 

Name 

Field 

Type 

Range 

Initial 

Log 

Comment 

fpOmax 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  0  position  maximum  flag 

fpOmin 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  0  position  minimum  flag 

fpOool 

result 

nag 

TRUE  II  FALSE 

FALSE 

1 

joint  0  position  out  of  limit 

pOmax 

set  point 

double 

31  jO 

c 

joint  0  maximum  position  (deg) 

pOmin 

set  point 

double 

-31.0 

c 

joint  0  minimum  position  (deg) 

Logic 

if(  pO  >  pOmax  ) 
else 

if(pO<pOmin) 
else 

if(  fpOmax  II  fpOmin 
else 

fpOmax  =  TRUE; 
fpOmax  =  FALSE; 
fpOmin  =  TRUE; 
fpOmin  ■  FALSE; 
fpOool  =  TRUE; 
fpOool  =  FALSE; 

Assignment  Parameters 

Name 

Assign  to 
Parameter 

Range 

Initial 

Log 

Comment 

pOhome 

pOsp 

<  pOmax  &&  >  pOmin 

0.0 

c 

home  position  for  joint  0  (deg) 

pi  home 

plsp 

<  plmax  &&  >  plmin 

Of) 

c 

home  position  for  joint  1  (deg) 

p2home 

p2sp 

<  p2max  &&  >  p2min 

SiS 

c 

home  position  for  joint  2  (cm) 

System  Parameters 

Name 

Type 

Range 

Initial 

Log 

Comment 

ftrack 

flag 

TRUE  11  FALSE 

FALSE 

tracking  control  mode  flag 

fjoy 

flag 

true  ii  falsi; 

FALSE 

joy  stick  control  mode  flag 

Figure  6.6  Joint  0  out-of-limit  model  and  assignment  and  system  parameters  defined  for  control 
sequences. 


parameter  was  set  to  true  to  activate  the  vision  control  mode.  The  fjoy  system  parameter  was 
set  to  true  to  activate  velocity  control  using  joy  stick  feedback.  Setting  the  system  parameter  to 
false  deactivated  the  control  mode. 

The  command  state  was  programmed  to  act  as  a  central  branching  node  to  different 
control  sequences.  Evaluation  of  system  parameters  were  made  to  determine  which  control 
sequence  was  activated.  The  command  state  established  position  control  for  each  robot  joint  and 
specified  home  locations  as  position  set  points.  The  ftrack  and  fjoy  system  parameters  were  set 
to  FALSE  during  initialization  to  ensure  that  the  command  state  remained  active  until  one  of 
these  system  parameters  was  set  to  TRUE  through  the  user  interface.  Tests  on  the  ftrack  and 
fjoy  system  parameters  were  made  to  branch  to  the  appropriate  control  sequence. 

Developers  of  joint  output  drivers  used  the  command  state  to  tune  position  feedback 
gains  since  position  control  was  utilized  on  joints  0, 1,  and  2.  Velocity  control  on  all  three  joints 
was  programmed  in  the  joy_stick  state  and  the  user  could  move  the  joy  sticks  to  alter  velocity 
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set  points  in  real  time.  The  joy_stick  state  was  used  to  tune  velocity  feedback  gains.  The 
feedback  control  algorithms  for  the  velocity  and  joy  stick  control  modes  were  identical  except 
that  with  joy  stick  control,  the  velocity  set  points  were  altered  in  real  time  using  joy  stick 
position  offset  information.  Vision  control  was  programmed  on  joints  0  and  1  in  the  track  state. 
The  track  state  was  used  to  tune  vision  feedback  gains.  The  track  state  established  vision 
control  on  joints  0  and  1  and  established  position  control  on  joint  2.  The  position  set  point  for  joint 
2  was  altered  through  the  user  interface  to  evaluate  tracking  dynamics  at  different  object 
distances. 

The  developers  of  joint  output  drivers  altered  feedback  gains  through  the  user  interface 
and  observed,  documented,  and  analyzed  joint  response  to  different  control  set  points.  The 
capacity  to  alter  feedback  gains  through  the  user  interface  was  removed  after  proper 
adjustment  of  feedback  gains  was  made.  The  control  sequences  were  not  removed  from  the 
program  since  the  track  and  joy_stick  states  were  used  for  demonstration  purposes. 

When  the  joy_stick  or  track  state  was  terminated,  a  systematic  method  to  return  joints 
to  home  locations  was  needed  before  the  command  state  was  activated.  The  recovery  sequence 
was  similar  to  the  power  up  sequence  except  home  locations  were  used  instead  of  mid  locations. 
The  home_pull_in  state  moved  joint  2  to  its  home  position  and  maintained  joints  0  and  1  at 
their  current  positions.  After  joint  2  reached  the  home  location  and  was  moving  slowly,  joints  0 
and  1  were  moved  to  their  home  locations  in  the  go_home  state.  When  all  three  joints  were  at 
their  home  locations,  the  command  state  was  activated. 

Picking  Sequence 

A  picking  sequence  was  programmed  to  pick  unobstructed  and  relatively  motionless 
fruit.  Additional  states  and  models  were  added  to  address  significant  problems  identified 
during  grove  operation.  Situations  encountered  during  grove  operation  that  were  addressed  by 
the  picking  program  were  1)  attempting  to  pick  out-of-reach  fruit,  2)  excessive  fruit  motion,  3) 
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fruit  that  vanished  from  view,  4)  picking  partially  occluded  fruit,  and  5)  collision  between  the 
robot  and  tree  structures. 

The  Basic  Picking  Sequence 

A  basic  picking  sequence  involved  locating  a  fruit,  aligning  the  picking  device  with  the 
fruit,  extending  toward  the  fruit,  detaching  the  fruit  when  proximity  was  detected, 
withdrawing  from  the  canopy,  and  removing  the  fruit  from  the  picking  device.  The  states 
programmed  for  the  basic  picking  sequence  were  extracted  from  the  state  planning  form 
(Appendix  E)  and  the  state  network  map  (Appendix  F)  and  are  presented  in  Figure  6.7.  States 
from  the  power  up,  power  down,  and  control  sequences  were  not  shown  in  this  state  network  map 
(with  the  exception  of  the  command  state)  to  conserve  space.  In  the  state  network  map  and  the 
state  network  maps  that  follow,  system  and  set  point  parameters  not  altered  by  the  displayed 
states  were  eliminated  from  the  figure  to  conserve  space.  The  reader  is  encouraged  to  observe 
the  complete  state  network  map  in  Appendix  F. 

Models  and  assignment  and  system  parameters  defined  for  the  basic  picking  sequence 
are  shown  in  Figure  6.8.  Models  were  programmed  to  determine  when  an  object  (located  by  the 
vision  input  driver)  was  centered  in  the  image  and  to  detect  when  the  end-effector  was  close  to 
an  object.  The  alignment  model  was  programmed  to  determine  when  the  end-effector  and  an 
object  were  aligned  and  used  two  set  point  parameters  to  established  centroid  error  limits.  The 
magnitude  of  the  difference  between  vision  set  points  and  object  centroids  were  compared  to 
vision  tolerance  criteria.  Both  the  horizontal  and  vertical  centroid  error  magnitudes  had  to  be 
less  than  the  tolerance  criteria  before  the  model  set  the  result  flag  (fvislop)  to  true.  Relative 
closeness  of  the  end-effector  to  an  object  was  determined  with  a  proximity  model.  The 
proximity  model  examined  a  result  from  the  distance  input  driver  (distp2)  and  set  a  result 
parameter  to  true  if  the  distance  to  an  object  was  less  than  or  equal  to  the  distsp  model  criterion. 

Assignment  parameters  (Figure  6.8)  were  defined  for  use  during  the  basic  picking 
sequence.  The  pmdelay  parameter  was  used  to  specify  the  timer  model  criterion  to  allow 
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State  Name 

Alias  N'ames 

Comment 

approach 

Move  joint  2  outward  using  velocity  control  while  using  vision  control  on  joint  0  and  1  to  maintain  end-effector — 
object  alignment. 

command 

Using  position  feedback  control,  move  all  three  joints  to  home  locations.  Test  status  of  three  system  flags  to 
determine  which  control  sequence  will  be  activated. 

detach 

Use  position  control  to  stop  robot  motion  and  activate  picking  device  to  closed  location  to  grasp  object. 

drop 

Activate  picking  device  to  open  location  while  maintaining  current  joint  positions  on  joints  0, 1,  and  2. 

fruit^check 

Stop  robot  motion  and  determine  if  an  object  can  be  located  in  image.  If  an  object  is  located,  activate  point  state. 

leave  pick 

Using  velocity  control,  move  joint  2  to  home  location.  Using  position  control,  keep  joints  0  and  1  at  current 
locations.  When  joint  2  reaches  position  set  point,  activate  command  state. 

pick_pull_in 

Using  position  control  on  all  three  joints,  move  joint  2  to  home  location  while  keeping  joints  0  and  1  at  current 
locations.  Activate  fruit  check  state  when  joint  2  is  close  to  position  set  point. 

point 

Use  vision  feedback  on  joints  0  and  1  to  align  end  effector  on  object  located  in  image.  Maintain  joint  2  at  current 
position. 

retract 

Move  joint  2  to  home  location  using  velocity  feedback  keeping  joints  0  and  1  at  current  locations. 

vision_touch 

Use  vision  control  on  joints  0  and  1  for  final  approach  to  object. 
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Figure  6.7  State  planning  form  and  state  network  map  for  basic  picking  sequence. 


complete  activation  of  the  picking  device  to  the  closed  or  open  location.  The  v2fast,  v2medium, 
and  v2slow  assignment  parameters  were  used  to  specify  outward  velocities  for  joint  2.  The  v2ret 
assignment  parameter  was  used  to  specify  the  velocity  set  point  for  retracting  the  end-effector 
from  the  canopy.  Image  mid  positions  had  been  defined  for  output  drivers  to  establish  vision 
set  points.  A  system  parameter  (fpick)  was  defined  to  enable  the  user  to  specify  when  the 
picking  sequence  was  activated. 
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Model  Name:  Alignment 

Function:  Determined  when  the  magnitude  of  horizontal  and  vertical  centroid  errors  were  below  an  allowable  limit.  Set  fvislop  to  true  if  both 

magnitudes  of  centroid  errors  were  below  centroid  tolerances  (viOslop  and  vilslop),  otherwise  set  fvislop  to  false. 


Name 

Field 

TyF* 

Range 

Initial 

Log 

Comment 

fvislop 

result 

flag 

TRUE  1  FALSE 

FALSE 

1 

centroid  slop  flag 

viOslop 

set  point 

long  int 

>0 

50 

c 

vertical  error  limit  (pixels) 

vilslop 

set  point 

long  int 

>0 

35 

c 

horizontal  error  limit  (pixels) 

Logic 

if(fblob  &&  (ABS(vilsp  -  xcent)  <  vilslop)  &&  (ABS(vi0sp  -  ycent)  < 
else 

viOslop)) 

fvislop 
fvislop 

-TRUE; 
*  FALSE; 

Model  Name: 
Function: 

Proximity 

Determined  when  an  object  was  close  to  the  ultrasonic  device.  Set  fprox  to  true  if  distance  to  object  (distp2)  was  less  than  or  equal  than 
distance  set  point  (distsp),  otherwise  set  fprox  to  false. 

Name 

Field 

Type 

Range 

Initial 

Log 

Comment 

fprox 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

proximity  flag 

distsp 

set  point 

double 

>=  4.0  &&  <=  80.0 

11.0 

c 

distance  set  point 

Logic 

if(distp2<=  distsp) 
else 

fprox  =  TRUE; 
fprox  =  FALSE; 

Assignment  Parameters 

Name 

Assign  to 
Parameter 

Range 

Initial 

Log 

Comment 

pmdelay 
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>=0 
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c 

picking  device  time  delay 

v2ret 

v2sp 
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return  velocity 

v2fast 

v2sp 
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c 

fast  outward  velocity 
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45.7 

c 

middle  outward  velocity 

v2slow 
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15.7 

c 

slow  outward  velocity 
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c 

mid  value  for  vision  vertical  (pixels) 
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c 
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Initial 
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TRUE  II  FALSE 

FALSE 
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Figure  6.8  Alignment  and  proximity  models  and  assignment  and  system  parameters  defined  for 
the  basic  picking  sequence. 


An  additional  link  was  programmed  in  the  command  state  to  test  the  status  of  the  fpick 
parameter.  If  fpick  evaluated  true,  the  pick_pull_in  state  was  activated.  The  pick_pull_in 
state  moved  joint  2  to  its  home  location  while  maintaining  joint  0  and  1  at  their  current 
positions.  If  fpick  evaluated  false  while  the  pick_pull_in  state  was  active,  the  leave_pick 
state  was  activated.  The  leave_pick  state  moved  joint  2  to  its  home  location  and  returned 
control  to  the  command  state  when  joint  2  was  close  to  its  position  set  point.  Each  state 
programmed  in  the  picking  sequence  included  a  test  on  the  fpick  parameter.  When  the  user  set 
fpick  to  false  through  the  user  interface,  the  picking  sequence  state  that  was  active  was 
deactivated  and  the  leave_pick  state  was  activated.  If  the  fpick  system  parameter  evaluated 
true,  the  link  to  the  fruit_check  state  was  evaluated. 
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The  fruit_check  state  was  activated  when  joint  2  reached  its  home  location.  The 
fruit_check  state  established  position  control  on  all  three  joints  and  current  joint  positions  for 
position  set  points.  If  an  object  was  located  in  the  image  (fblob  result  parameter  from  the  vision 
input  driver  software  evaluated  true),  the  point  state  was  activated.  The  point  state 
established  vision  control  on  joints  0  and  1  with  vision  set  points  established  at  image  mid 
locations  and  position  control  on  joint  2  with  a  position  set  point  established  at  the  current 
location  of  joint  2.  Using  vision  feedback,  the  end-effector  was  aligned  on  the  object.  When 
end-effector  alignment  was  adequate  (fvislop  evaluated  true),  the  approach  state  was 
activated. 

The  approach  state  utilized  vision  control  on  joints  0  and  1  to  maintain  alignment  and 
velocity  control  on  joint  2  to  extend  the  end-effector  into  the  canopy.  Image  mid  locations 
(viOmid  and  vilmid)  were  used  as  vision  set  points  for  joints  0  and  1.  The  v2fast  assignment 
parameter  was  used  to  specify  joint  2's  velocity  set  point.  When  proximity  to  an  object  was 
detected,  the  vision_touch  state  was  activated.  The  vision_touch  state  moved  joint  2  to  a 
picking  position  at  a  slower  outward  velocity  (assignment  parameter  v2medium)  and 
maintained  vision  control  on  joints  0  and  1.  The  pick  position  result  from  the  distance  input 
driver  (p2pick)  was  used  to  establish  joint  2's  position  set  point.  This  final  approach  moved 
the  picking  device  closer  to  the  object.  Notice  that  velocity  control  was  used  to  move  the  end- 
effector  outward  but  the  link  to  the  detach  state  evaluated  the  result  from  a  position  error 
model.  Due  to  design  of  the  robot,  joint  2  responded  better  under  velocity  feedback  for  small 
motion  control. 

When  the  picking  position  had  been  achieved  (fp2slop  evaluated  true),  the  detach 
state  was  activated.  The  detach  state  utilized  position  control  on  all  three  joints  with  current 
positions  used  for  position  set  points  to  stop  robot  motion  and  activated  the  picking  device  to 
the  closed  location.  A  time  delay  (pmdelay)  was  required  to  allow  for  complete  activation  of 
the  picking  device.  When  the  picking  device  time  interval  had  elapsed  (fdt  evaluated  true), 
the  retract  state  was  activated.  The  retract  state  established  position  control  on  joints  0  and  1 
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and  velocity  control  on  joint  2.  The  velocity  set  point  for  joint  2  was  established  at  a  return 
value  (v2ret)  to  pull  the  end-effector  out  of  the  canopy.  As  in  the  vision_touch  state,  velocity 
control  was  utilized  to  retract  the  end-effector  while  the  position  of  joint  2  determined  when 
the  next  state  was  activated.  When  the  position  of  joint  2  was  close  to  its  home  location,  the 
drop  state  was  activated.  The  drop  state  established  position  control  on  joints  0, 1,  and  2  and 
used  the  current  positions  of  joints  0  and  1  and  joint  2's  home  location  for  position  set  points.  The 
picking  device  was  activated  to  the  open  location  for  fruit  removal.  After  the  picking  device 
time  delay,  the  pick_pull_in  state  was  activated.  With  the  return  to  the  pick_pull_in  state, 
the  basic  picking  sequence  was  completed. 

Search  Sequence 

If  an  object  could  not  be  located  in  the  image  while  the  fruit_check  state  was  active, 
the  fruit_check  state  remained  active.  To  initiate  a  search  for  fruit,  a  search  sequence  was 
programmed  to  systematically  cycle  joints  0  and  1  through  a  search  pattern  while  monitoring 
the  fblob  result  from  the  vision  input  driver.  If  an  object  was  located  while  the  search  sequence 
was  active,  the  picking  sequence  was  re-entered  by  activation  of  the  point  state.  The  state 
planning  form  and  state  network  map  for  the  search  sequence  is  shown  in  Figure  6.9. 

The  assignment  and  system  parameters  and  macros  defined  for  the  search  sequence  are 
shown  in  Figure  6.10.  The  system  parameters  fdirO  and  fdirl  were  programmed  to  indicate  the 
direction  to  move  joints  0  and  1.  Four  macros  were  defined  to  specify  the  status  of  the  two 
direction  flags.  The  UP  and  DOWN  macros  were  defined  to  specify  the  value  of  the  fdirO 
parameter  and  the  RIGHT  and  LEFT  macros  were  defined  to  specify  the  value  of  the  fdirl 
parameter.  A  system  parameter  (wait)  was  defined  to  delay  reaction  of  fruit  detection  at  the 
beginning  of  the  search  sequence  so  the  end-effector  was  moved  away  from  the  current  position. 

Assignment  parameters  were  defined  to  limit  the  search  volume  for  joints  0  and  1  and  to 
specify  joint  search  direction  and  speed.  Search  position  limits  (pOup,  pOdn,  plrt,  and  pllf) 
were  defined  to  limit  joint  0  and  l's  search  volume.  Velocity  specifications  (vOup,  vOdn,  vlrt, 
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state  a  a  me 

Alias  Names 

Comment 

abort_pick 

Abort  current  picking  activity  by  moving  joint  2  to  home  location  using  position  control  and  maintaining  joints  0 
and  1  at  current  locations.  Test  direction  flags  to  determine  which  direction  joints  0  and  1  should  be  moved  in  search 
for  the  next  object  to  pick. 

downjeft 

Using  velocity  control,  move  joint  0  downward  and  joint  1  to  the  left  while  checking  for  fruit.  Use  position  control 
on  joint  2  and  keep  joint  2  at  current  position. 

down_right 

Using  velocity  control,  move  joint  0  downward  and  joint  1  to  the  right  while  checking  for  fruit.  Use  position  control 
on  joint  2  and  keep  joint  2  at  current  position. 

upjeft 

Using  velocity  control,  move  joint  0  upward  and  joint  1  to  the  left  while  checking  for  fruit.  Use  position  control  on 
joint  2  and  keep  joint  2  at  current  position. 

up_right 

Using  velocity  control,  move  joint  0  upward  and  joint  1  to  the  right  while  checking  for  fruit.  Use  position  control 
on  joint  2  and  keep  joint  2  at  current  position. 
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Figure  6.9  State  planning  form  and  state  network  map  for  search  sequence. 


and  vllf)  were  defined  to  establish  velocity  set  points.  Through  the  user  interface,  the  user 
changed  the  value  of  these  parameters  to  alter  the  search  pattern  on-line.  An  assignment 
parameter  (waitdef)  was  defined  to  establish  the  default  value  for  the  wait  system 
parameter. 

The  fruit_check  state  was  included  in  the  state  network  map  (shown  in  Figure  6.9)  to 
show  how  this  state  was  altered  to  accommodate  the  search  sequence.  If  an  object  was  not 
located  in  the  image  during  the  fruit_check  state,  the  abort_pick  state  was  activated  to 
withdraw  the  end-effector  from  the  canopy.  The  abort_pick  state  was  used  to  terminate  a  pick 
cycle  for  a  variety  of  reasons  that  are  discussed  in  later  sections.  The  abort_pick  state  moved 
joint  2  to  its  home  location  and,  depending  on  the  status  of  the  joint  direction  flags  (fdirO  and 
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Assignment  Parameters 

Name 

Assign  to 
Parameter 

Range 

Initial 

Log 

Comment 
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Figure  6.10  Assignment  and  system  parameters  and  macro  substitutions  defined  for  search 
sequence. 


fdirl),  activated  a  search  sequence  state.  Each  state  in  the  search  sequence  used  velocity 
control  on  joints  0  and  1  with  velocity  set  points  established  in  a  direction  indicative  of  the 
state  name.  Position  set  points  were  established  at  search  direction  limits.  Tests  on  results  from 
position  error  and  out-of-limit  models  were  used  to  activate  another  state  in  the  search 
sequence  to  switch  search  directions. 

For  example,  the  downjeft  state  established  joint  0's  position  set  point  to  a  lower 
search  limit  (pOdn),  joint  l's  position  set  point  to  a  left  search  limit  (pllf),  joint  0's  velocity  set 
point  to  a  downward  velocity  (vOdn),  and  joint  l's  velocity  set  point  to  a  leftward  velocity 
(vllf).  The  value  of  the  direction  flags  fdirO  and  fdirl  were  altered  to  reflect  the  down-left 
motion.  The  down_right  state  was  activated  if  the  left  search  limit  was  reached  and  the 
upjeft  state  was  activated  if  the  lower  search  limit  was  reached.  If  an  object  was  located 
during  the  search  sequence,  the  point  state  was  activated.  If  the  fpick  parameter  was  set  to 
FALSE,  the  sequence  was  terminated  by  activation  of  the  leave_pick  state.  When  a  state  in 
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the  search  sequence  state  was  activated,  the  direction  flags  were  altered  to  indicate  the  current 
direction  of  motion  for  joints  0  and  1 .  The  last  direction  of  joint  motion  were  saved  for  subsequent 
activation  of  the  search  sequence. 

After  aborting  an  attempt  to  pick  an  unreachable  fruit,  the  same  fruit  was  located  by 
the  vision  input  driver.  Another  attempt  to  pick  the  fruit  occurred  if  the  picking  sequence  was 
immediately  restarted.  An  attempt  to  eliminate  this  situation  was  made  by  incorporation  of  a 
time  delay.  When  the  first  search  sequence  state  was  activated  (depending  on  the  status  of  the 
joint  direction  flags),  object  information  was  not  considered  valid  until  a  time  interval  had 
elapsed.  The  picking  device  was  moved  away  from  the  current  location  as  a  result  of  the  time 
delay.  If  the  picking  device  was  cycled  back  near  the  same  location  of  the  out-of-reach  fruit, 
another  attempt  to  pick  the  fruit  was  made.  This  created  a  problem  if  the  robot  was  operated 
in  a  stationary  mode.  When  the  robot  was  moved  slowly  through  the  grove,  fruit  that  were 
out-of-reach  either  came  within  robot  reach  or  were  no  longer  visible.  The  problem  of  repeated 
pick  cycles  on  fruit  that  could  not  be  reached  when  the  robot  was  operated  in  the  stationary 
mode  was  not  addressed. 

A  system  parameter  (wait)  was  defined  to  establish  a  time  interval  for  the  first  search 
sequence  state  that  was  activated  after  an  aborted  pick  cycle.  The  abort_pick  state  initialized 
the  wait  parameter  to  a  default  value  (system  parameter  waitdef).  The  search  sequence  states 
initialized  the  timer  model  criterion  (dttime)  to  the  value  of  the  wait  parameter  and  zeroed 
the  wait  parameter  for  the  next  search  sequence  state.  The  next  search  sequence  state  used  the 
zeroed  wait  parameter  for  the  timer  model  criterion.  Thus,  only  the  first  search  sequence  state 
activated  after  the  abort_pick  state  delayed  response  to  a  fruit  located  by  the  vision  input 
driver. 

Detection  of  Out-of-Reach  Fruit 

Several  situations  occurred  during  the  basic  picking  sequence  that  prevented  effective 
fruit  removal.  When  vision  control  was  used  on  joints  0  and  1  to  align  the  end-effector  on  a  fruit 
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located  outside  the  robot's  work  space,  joints  0  or  1  were  moved  toward  the  work  space  limit. 
When  a  work  space  limit  was  exceeded,  the  power  down  sequence  was  initiated  in  response  to  a 
safety  link.  Intelligence  was  incorporated  to  recognize  the  approach  of  minimum  and  maximum 
position  limits  by  using  results  from  the  joint  out-of-limits  model  programmed  for  the  track 
state.  Tests  on  the  model's  results  were  added  to  the  point,  approach,  vision_touch,  and  detach 
states.  The  abort_pick  state  was  activated  when  a  maximum  or  minimum  work  space  limit  was 
exceeded.  Alias  names  were  used  to  indicate  which  joint  caused  the  activation  of  the 
abort_pick  state.  For  example,  if  joint  1  exceeded  a  maximum  position  limit,  the  abort_pick 
state  was  activated  using  the  ap_plmax  alias  name.  Changes  to  the  basic  picking  sequence  to 
determine  joint  out-of-limits  are  shown  in  Figure  6.11. 
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Figure  6.11  Picking  sequence  state  network  map  altered  to  detect  out-of-limits. 


Detection  of  Excessive  Fruit  Motion 


Another  problem  encountered  during  grove  operation  was  excessive  fruit  movement. 
While  the  robot's  tracking  ability  compensated  for  some  fruit  movement,  attempts  to  pick  fruit 
that  had  excessive  motion  were  unsuccessful.  The  first  indication  that  a  fruit  had  motion  was 
when  end-effector  alignment  with  a  fruit  was  not  maintained.  A  second  indication  of  fruit 
motion  was  when  joints  0  or  1  had  large  velocity  measurements  when  maintaining  end-effector 
alignment.  The  changes  to  the  basic  picking  sequence  to  recognize  excessive  fruit  motion  are 
shown  in  Figure  6.12. 
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Figure  6.12  Picking  sequence  state  network  map  altered  to  detect  moving  fruit. 


The  slow_out  state  was  programmed  to  reduce  the  outward  velocity  of  the  end-effector 
when  alignment  with  a  fruit  was  not  maintained.  The  slow_out  state  was  similar  to  the 
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approach  state  except  a  slower  outward  velocity  was  used  for  joint  2's  velocity  set  point.  The 
slower  outward  velocity  of  the  end-effector  provided  an  increased  time  interval  for  the  control 
algorithms  used  for  joints  0  and  1  to  compensate  for  fruit  movement.  If  end-effector  alignment 
was  established  during  the  slow_out  state,  the  approach  state  was  activated. 

Results  from  the  velocity  model  were  used  to  indicate  when  the  velocity  of  joints  0  or  1 
were  large.  False  results  from  this  model  indicated  that  the  velocity  magnitude  of  a  joint  was 
above  model  criterion.  When  a  large  velocity  was  detected  on  joints  0  or  1,  the  wait_velocity 
state  was  activated.  The  wait_velocity  state  was  programmed  to  stop  outward  motion  of  the 
end-effector  until  the  velocities  of  joints  0  and  1  were  within  modeling  criteria.  The 
wait_velocity  state  also  tested  the  result  from  the  alignment  model.  The  approach  state  was 
activated  when  alignment  was  established  and  the  velocity  magnitudes  of  joints  0  and  1 
returned  to  acceptable  levels. 

Detection  of  Vanishing  Fruit 

Another  factor  which  complicated  picking  fruit  was  fruit  located  inside  the  canopy. 
When  vision  control  was  used  for  joints  0  and  1,  the  possibility  existed  that  the  vision  input 
driver  was  not  able  to  detect  a  valid  object.  Loss  of  object  detection  resulted  from  several 
different  situations.  Several  of  these  are  identified  and  summarized  below. 

During  alignment  of  the  end-effector  on  a  fruit,  a  different  viewing  angle  of  the  canopy 
resulted.  If  a  leaf  was  located  between  the  camera  and  the  fruit,  the  different  view  may  cause 
the  fruit  diameters  to  fall  below  the  minimum  required  for  valid  object  detection.  This  is  shown 
in  Figure  6.13.  Forward  movement  of  the  trailer  also  resulted  in  a  different  canopy  view.  In 
some  cases,  a  visible  fruit  became  totally  occluded  by  tree  structures  during  a  pick  cycle.  Image 
quality  as  affected  by  aperture  control  and  background  conditions  also  affected  the  object 
location  ability  of  the  vision  input  driver.  Another  situation  that  resulted  in  losing  sight  of  a 
fruit  was  fruit  movement.  If  the  robot  did  not  have  adequate  dynamic  response  to  track  a 
moving  fruit,  the  fruit  moved  out  of  the  camera's  field  of  view.  Although  excessive  fruit  motion 


Figure  6.13  Loss  of  object  identification.  A)  Initial  targeting.  B)  Alignment  on  object. 

was  handled  by  the  wait_velocity  state,  fruit  motion  toward  and  away  from  the  robot  altered 
the  perceived  fruit  diameters.  As  the  fruit  moved  away  from  the  camera,  the  vision  input 
driver  no  longer  classified  the  fruit  as  valid  because  of  the  reduced  fruit  diameter 
measurements. 

Two  solutions  to  the  vanishing  fruit  problem  were  implemented.  The  first  abandoned 
the  use  of  the  fblob  parameter  as  an  indicator  that  a  fruit  was  located  in  the  image  and  used  a 
result  of  a  model  that  averaged  the  fblob  result  over  several  image  analyses.  This  solved 
intermittent  causes  of  vanishing  fruit  due  to  changes  in  image  illumination.  The  second  solution 
was  to  obtain  a  closer  view  of  the  canopy  and  was  implemented  by  adding  an  additional  state 
to  the  picking  sequence.  The  changes  to  the  basic  picking  sequence  are  shown  in  Figure  6.14. 

The  vision  series  model  (Figure  6.15)  calculated  a  running  average  of  the  fblob  result. 
The  running  average  was  accomplished  by  saving  the  current  value  of  the  fblob  result  in  a  local 
storage  array.  If  the  sum  of  the  local  array  was  equal  to  or  greater  than  a  model  criterion,  the 
fviser  result  parameter  was  set  to  true.  This  model  made  use  of  the  fact  that  a  Boolean  true 
value  was  equivalent  to  a  numerical  1  and  a  false  value  was  equivalent  to  a  numerical  0.  The 
picking  sequence  states  were  changed  by  altering  any  reference  to  the  vision  input  result  fblob  to 
the  model  result  fviser. 
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Figure  6.14  Picking  sequence  state  network  map  altered  to  detect  lost  fruit. 


Model  Name: 
Function: 

Vision  series 

Determined  when  the  vision  input  module  had  located  an  object  for  a  certain  number  of  times  out  of  the  past  10  control  cycles.  Set 
fviser  to  true  if  an  object  had  been  located  a  designated  number  of  times  (vilen)  out  of  the  past  10  control  cycles,  otherwise  set  fviser  to 
false. 
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Figure  6.15  Vision  series  model. 


If  the  vision  series  model  reported  loss  of  object  identification,  the  pick  cycle  should  be 
aborted.  Before  this  extreme  was  taken,  it  was  decided  that  a  closer  view  of  the  canopy  may 
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increase  the  probability  of  relocating  the  object.  The  wait_pick  state  was  developed  to  slowly 
move  the  end-effector  into  the  canopy.  Position  control  was  established  on  joints  0  and  1  with 
the  current  positions  utilized  as  set  points.  Velocity  control  on  joint  2  was  utilized  with  a  slow 
outward  velocity  set  point  to  obtain  a  closer  view  of  the  canopy.  A  position  set  point  for  joint  2 
was  established  a  short  distance  beyond  its  current  position  when  a  fruit  was  lost.  If  a  valid 
fruit  was  located  during  the  wait_pick  state,  the  approach  state  was  activated.  The  pick  cycle 
was  aborted  by  activation  of  the  abort_pick  state  if  either  joint  2  reached  its  position  set  point, 
proximity  to  an  object  was  detected,  or  the  wait_pick  state  was  active  for  two  seconds. 

Detection  of  Partially  Occluded  Fruit 

Another  problem  encountered  during  grove  operation  was  attempting  to  pick  fruit  that 
were  partially  occluded.  As  the  picking  device  was  extended  toward  a  fruit,  a  leaf  or  limb 
located  in  front  of  the  fruit  was  targeted  by  the  distance  sensor.  To  successfully  pick  partially 
occluded  fruit,  it  was  necessary  to  determine  if  the  distance  sensor  was  targeting  fruit  or  a  tree 
structure.  When  the  end-effector  was  close  to  an  object,  a  partially  occluded  fruit  was 
characterized  by  small  diameters.  This  could  be  the  result  of  a  small  fruit  located  in  front  of 
the  ultrasonics  or  a  fruit  being  obscured  by  leaves  and  limbs.  The  changes  made  in  the  basic 
picking  sequence  to  detect  partially  occluded  fruit  are  shown  in  Figure  6.16. 

A  diameter  model  (Figure  6.17)  was  developed  to  compare  fruit  diameters  to  diameter 
set  points.  The  diameter  model  set  the  fpickd  result  if  one  of  the  fruit  diameters  met  or 
exceeded  model  criteria.  The  picking  sequence  was  executed  as  previously  discussed  if  the 
result  from  the  diameter  model  was  true  when  proximity  to  an  object  was  detected.  If  the  result 
was  false,  the  diameters  of  the  fruit  were  small  implying  that  the  distance  sensor  was  not 
measuring  the  distance  to  the  fruit.  When  this  occurred,  the  occluded  state  was  entered.  The 
occluded  state  slowly  extended  the  end-effector  into  the  canopy  while  maintaining  alignment 
on  the  fruit.  A  position  set  point  for  joint  2  was  established  to  limit  how  far  the  picking  device 
was  extended  into  the  canopy.  The  distance  to  extend  into  the  canopy  was  specified  using  the 
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Figure  6.16  Picking  sequence  state  network  map  altered  to  detect  partially  occluded  fruit. 


Model  Name: 

Diameter 

Function: 

Determined  when  the  horizontal  or  vertical  diameter  of  an  object  exceeds  diameter  set  points.  Set  fpickd  to  true  if  either  the  horizontal 
diameter  (xdia)  was  greater  than  or  equal  to  horizontal  picking  diameter  set  point  (vilpickd)  or  the  vertical  diameter  (ydia)  was  greater 
than  or  equal  to  vertical  picking  diameter  set  point  (viOpickd),  otherwise  set  fpickd  to  false. 

Name 

Field 

Type 
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Log 
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fpickd 

result 
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FALSE 

1 

diameter  flag 

viOpickd 

set  point 

long  int 
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c 

model  y  diameter 
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long  int 

<  (xmax-xmin) 
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c 

model  x  diameter 

Logic         Iff  fblob&4((xdia>=  vilpickd)  II  (ydia  >=  viOpickd)))          fpickd  a  TRUE; 

else                                                                      fpickd  ■  FALSE; 

Figure  6.17  Diameter  model. 


system  parameter  occdist  (see  Appendix  B).  The  occluded  distance  was  added  to  the  current 
position  of  joint  2  to  establish  the  position  set  point. 
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If  a  valid  object  was  reported  by  the  vision  series  model  and  the  distance  model 
reported  that  an  object  was  not  in  proximity  with  the  picking  device,  the  approach  state  was 
activated.  If  the  fruit  was  lost  from  view  during  the  occluded  state,  the  touch  state  was 
utilized  to  use  a  dead  reckoning  extension  into  the  canopy  and  then  the  detach  state  was 
activated  to  complete  the  pick  cycle.  This  situation  occurred  when  an  object  was  very  close  to 
the  end-effector  and  occluded  light  to  the  camera.  Two  assignment  parameters  (viOppd  and 
vilppd)  were  defined  for  use  in  setting  diameter  model  criteria  during  the  occluded  state.  The 
value  of  these  parameters  represented  diameters  of  a  fruit  when  a  fruit  was  very  close  to  the 
end-effector.  If  one  of  the  fruit  diameters  exceeded  the  diameter  model  criteria  or  joint  2  had 
traveled  the  specified  distance,  the  detach  state  was  activated.  Two  alias  names  were  defined 
for  the  detach  state  so  indicate  the  reason  the  detach  state  was  activated.  The 
diameter_detach  alias  was  used  to  indicate  that  the  detach  state  was  activated  because 
diameter  model  criteria  had  been  exceeded.  The  slop_detach  alias  was  used  to  indicate  that 
joint  2  was  close  to  its  position  set  point. 

Detection  of  Stuck  Condition  on  Joint  2 

During  extension  of  the  picking  device  toward  a  fruit,  there  was  a  possibility  of  a 
collision  between  the  robot  and  a  tree  structure  large  and  stiff  enough  that  impeded  outward 
movement.  Identification  of  this  situation  was  important  to  reduce  damage  to  the  robot  and 
tree  structures.  To  determine  when  outward  movement  of  joint  2  was  impeded,  a  stuck  model  was 
developed. 

The  stuck  model  (Figure  6.18)  monitored  the  servo-valve  control  word  for  joint  2.  The 
fstuck  result  parameter  was  set  when  the  magnitude  of  the  control  word  exceeded  model 
criterion  for  a  designated  time  interval.  The  result  from  the  stuck  model  was  utilized  in  two 
different  ways  in  the  picking  sequence.  Changes  to  the  basic  picking  sequence  to  recognize  the 
stuck  condition  are  shown  in  Figure  6.19.  If  joint  2  became  stuck  during  approach,  the  pick  cycle 
was  aborted  by  activation  of  the  abort_pick  state.  If  a  tree  structure  was  caught  in  the  closed 
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Model  Name:  Stuck 

Function:  Determined  when  the  magnitude  of  joint  2's  servo-valve  control  word  exceeds  an  allowable  limit  for  a  specified  time  interval.  Set 

fstuck  if  the  magnitude  of  the  control  word  was  greater  than  a  control  word  limit  (v2cwchk)  for  a  given  time  interval  <v2cwtim), 
otherwise  set  fstuck  to  false. 
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Figure  6.18  Stuck  model. 
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Figure  6.19  Picking  sequence  state  network  map  altered  to  detect  stuck  condition  on  joint  2. 
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picking  device,  retraction  of  joint  2  was  prevented.  The  release  state  was  programmed  to  open 
the  picking  device  and  activated  the  abort_pick  state  after  the  picking  device  time  interval. 
If  the  fstuck  parameter  was  set  while  the  end-effector  was  being  withdrawn  from  the  canopy, 
the  release  state  was  activated. 

End-Effector  Design  Problem 

Some  states  were  developed  to  overcome  inadequacies  in  the  robot  design.  The 
discussion  of  these  states  is  included  for  completeness.  After  the  drop  state  had  been  activated 
to  remove  a  fruit  from  the  end-effector,  leaves  or  fruit  sometimes  remained  in  the  picking 
device.  In  most  cases,  operation  of  the  picking  device  to  the  closed  and  then  to  the  open  location 
cleared  the  obstruction.  Adding  two  states  to  perform  this  function  automatically  after  each 
pick  cycle  was  accomplished  by  adding  the  lip_up  and  lip_down  states.  The  state  link  between 
the  drop  state  and  the  pick_pull_in  state  was  moved  to  the  lip_up  state.  The  lip_up  state 
activated  the  picking  device  to  the  closed  location.  After  the  picking  device  time  delay,  the 
lip_down  state  was  activated  to  open  the  picking  device.  The  pick_pull_in  state  was 
activated  after  the  picking  device  time  delay. 


CHAPTER  7 

EVALUATION  OF  PICKING  INTELLIGENCE  AND  PICKING  LANGUAGE 


This  chapter  evaluates  the  picking  intelligence  programmed  to  pick  fruit  grown  in 
production  conditions  and  evaluates  the  adequacy  of  the  picking  language.  The  picking 
intelligence  was  verified  by  recording  data  while  the  robot  picked  fruit.  Data  logged  at  a 
real-time  rate  included  joint  position  and  velocity,  the  distance  from  the  end-effector  to  an 
object,  object  diameters  and  centroids,  servo-valve  control  words,  joint  control  modes,  joint 
control  set  points,  and  model  results.  The  adequacy  of  the  picking  language  was  evaluated  by 
examining  the  ease  of  specifying,  modifying,  and  debugging  the  picking  intelligence. 

Picking  Intelligence  Evaluation 
To  verify  that  the  picking  intelligence  adequately  overcame  problems  encountered 
during  grove  operation,  data  were  logged  while  the  robot  picked  fruit.  When  a  pick  cycle 
occurred  that  illustrated  characteristics  of  problems  associated  with  grove  operation,  the  data 
were  transferred  to  a  mass  storage  device.  Data  plots  were  generated  using  graphic  software  on 
the  printer/plotter  console. 

Data  from  Execution  of  Search  Sequence 

A  search  sequence  was  used  to  systematically  move  joints  0  and  1  through  a  search 
volume  until  an  object  was  detected  in  the  image.  The  state  network  map  for  the  search 
sequence  was  presented  in  Figure  6.9  in  chapter  6.  To  document  the  search  pattern,  the  robot  was 
positioned  such  that  no  fruit  were  within  visual  range  and  the  fpick  system  parameter  was  set 
to  true  through  the  user  interface.  Since  a  fruit  could  not  be  located  during  the  picking  sequence, 
the  search  sequence  was  activated.  The  cycling  between  the  downjright,  downjeft,  up_right, 
and  upjeft  states  moved  the  end-effector  through  a  search  work  space.  Plots  of  position, 
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position  set  point,  velocity,  and  velocity  set  point  for  joints  0  and  1  and  the  state  execution 
sequence  are  shown  in  Figure  7.1.  The  horizontal  axis  represented  time  in  1/60  second.  The 
position  and  velocity  of  joint  2  were  not  shown  since  joint  2  remained  at  the  home  location  and  at 
zero  velocity. 
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Figure  7.1  Data  plots  for  search  sequence. 
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The  state  execution  sequence  shows  the  order  of  state  activation  by  indicating  when  a 
state  was  activated  with  how  long  the  state  was  active.  The  left  edge  of  a  bar  indicated  when 
a  state  was  activated.  The  right  edge  indicated  when  a  state  was  deactivated.  For  example, 
the  command  state  was  active  from  time  0  to  86  and  the  up_right  state  was  active  from  time  318 
to  384  and  from  time  450  to  501. 

The  data  recorded  to  document  the  search  sequence  started  while  the  command  state 
was  active.  The  fpick  system  parameter  was  set  to  true  through  the  user  interface  at  time  86 
resulting  in  the  activation  of  the  pick_pull_in  state.  The  pick_pull_in  state  moved  joint  2  to 
its  home  location  and  was  only  active  for  one  control  cycle  since  joint  2  was  already  at  its  home 
location.  The  fruit_check  state  was  activated  to  determine  if  an  object  was  located  in  the 
image.  Since  no  object  was  located,  the  abort_pick  state  was  activated  to  abort  the  picking 
sequence  and  initiate  the  search  sequence.  The  abort_pick  state  moved  joint  2  to  its  home 
location  and  then  determined  which  direction  to  move  joints  0  and  1.  Since  joint  2  had  not 
moved  from  its  home  location  the  abort_pick  state  was  only  active  for  one  control  cycle.  The 
status  of  the  fdirO  and  fdirl  system  parameters  indicated  which  state  in  the  search  sequence 
(down_left)  was  activated. 

The  downjeft  state  established  joint  velocity  feedback  control  and  velocity  set  points 
for  joints  0  and  1  to  move  the  end-effector  down  and  to  the  left.  Joint  0  moved  the  end-effector 
downward  toward  a  lower  search  limit  (-28  degrees)  while  joint  1  moved  the  end-effector 
toward  a  left  search  limit  (-8  degrees).  At  time  209,  the  position  of  joint  1  was  close  to  the 
position  set  point  and  the  joint  position  error  model  set  a  result  parameter  to  true.  The 
down_right  state  was  activated  to  switch  search  directions  on  joint  1.  Joint  0  continued  to  move 
toward  the  lower  search  limit  while  joint  1  now  moved  toward  a  right  search  limit  (-1 
degrees).  The  right  limit  was  reached  at  time  271  and  the  downjeft  state  was  activated  to 
change  the  search  direction  on  joint  1.  Joint  0  reached  its  lower  limit  at  time  279  and  the 
upjeft  state  was  activated  to  move  joint  0  toward  an  upper  search  limit  (23  degrees).  The 
cycling  between  up  and  down  on  joint  0  and  left  and  right  on  joint  1  continued  until  the  fpick 
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system  parameter  was  set  to  false  through  the  user  interface  at  time  752.  The  leave_pick  state 
was  activated  to  move  joint  2  to  its  home  location.  Since  joint  2  had  not  moved  during  the  search 
sequence,  the  command  state  was  activated  at  the  next  control  cycle  and  moved  joints  0  and  1  to 
their  respective  home  location. 

Picking  Fruit 

Data  were  continuously  recorded  by  the  data  logger  while  the  robot  picked  fruit.  Data 
were  saved  to  a  mass  storage  device  when  a  pick  cycle  was  executed  that  demonstrated  the 
ability  of  the  picking  intelligence  to  recognize  the  out-of-reach  condition,  excessive  fruit 
motion,  vanishing  fruit,  fruit  that  were  partially  occluded,  or  the  stuck  condition  on  joint  2. 
Data  from  five  pick  cycles  are  presented.  Data  display  were  used  for  two  levels  of  debugging. 
Data  from  input  and  output  driver  software  were  useful  to  determine  appropriate  numerical 
values  for  modeling  criteria.  Debugging  the  program  to  pick  fruit  required  observation  of  model 
results  and  a  state  execution  sequence. 

Plots  of  joint  position  and  velocity,  object  diameters  and  centroids,  and  the  distance  to 
an  object  are  shown  in  Figure  7.2  to  document  a  pick  cycle.  The  fruit  picked  during  this  attempt 
was  approximately  90  cm  in  front  of  the  robot,  relatively  motionless,  and  not  occluded  by 
leaves.  It  was  difficult  to  relate  these  data  to  what  happened  during  the  pick  cycle. 

To  debug  at  the  program  level,  display  of  modeling  results  and  the  state  execution 
sequence  were  used.  The  data  for  this  pick  cycle  are  shown  in  Figure  7.3.  The  position  of  joint  2 
best  documented  the  pick  cycle  since  outward  movement  of  the  end-effector  can  readily  be 
determined.  In  the  center  are  three  bar  graphs  representing  results  from  the  vision  series, 
alignment,  and  proximity  models.  A  true  fviser  result  from  the  vision  series  model  indicated 
that  an  object  had  been  located  in  the  image.  A  false  fvislop  result  from  the  alignment  model 
indicated  that  the  picking  device  was  not  aligned  with  an  object  located  in  the  image.  A  true 
fprox  result  from  the  proximity  model  indicated  that  an  object  was  close  to  the  ultrasonic 
transducer.  The  bar  graph  at  the  bottom  of  the  figure  shows  the  state  execution  sequence. 
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Figure  7.2  Position  and  velocity  of  joints  0, 1,  and  2  and  object  information  during  a  pick  cycle. 
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Figure  7.3  A  documented  pick  cycle. 
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Data  saved  to  document  this  pick  cycle  started  at  the  end  of  a  previous  pick  cycle  with 
the  lip_down  state  active.  After  the  picking  device  time  interval  had  expired,  the 
pick_pull_in  state  was  activated  to  move  joint  2  to  its  home  location.  The  fruit_check  state 
was  activated  to  determine  if  an  object  had  been  located  in  the  image.  A  valid  object  was 
detected  at  time  34  (vision  series  model  result  fviser  evaluated  true)  before  a  time  interval 
expired  resulting  in  activation  of  the  point  state.  The  point  state  used  vision  control  on  joints  0 
and  1  to  establish  alignment  between  the  end-effector  and  the  object.  Alignment  was 
established  at  time  44  and  the  approach  state  was  activated.  The  approach  state  established 
joint  2  velocity  control  with  an  outward  velocity  set  point  to  move  the  end-effector  toward  the 
object  and  retained  vision  control  on  joints  0  and  1  to  maintain  alignment  between  the  object  and 
the  end-effector. 

At  time  45,  a  false  result  from  the  vision  series  model  indicated  that  an  object  was  not 
located  and  the  wait_pick  state  was  activated  to  obtain  a  closer  view  of  the  canopy.  As  joint  2 
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was  moved  closer  to  the  canopy,  an  object  was  located  at  time  66  and  the  approach  state  was 
activated.  During  the  approach  state,  misalignment  with  the  object  resulted  in  activation  of 
the  slow_out  state.  The  slow_out  state  performed  the  same  actions  as  the  approach  state  but 
used  a  slower  outward  velocity  set  point  for  joint  2.  At  time  73,  both  horizontal  and  vertical 
centroids  were  within  limits  and  the  approach  state  was  activated.  The  slow_out  state  was 
activated  at  time  75  due  to  the  misalignment  between  the  object  and  the  end-effector. 

During  the  slow_out  state,  an  object  could  not  be  detected  (fviser  evaluated  false)  and 
the  wait_pick  state  was  activated  to  extend  the  end-effector  toward  the  canopy.  An  object  was 
located  at  time  81  and  the  approach  state  was  activated.  The  inability  of  joint  0  or  1  to 
maintain  alignment  with  the  object  caused  a  switch  to  the  slow_out  state  at  time  86.  The 
inability  to  maintain  alignment  was  caused  by  1 )  inadequate  tuning  on  the  alignment  model 
criteria,  2)  fruit  motion,  or  3)  inadequately  tuned  vision  controllers.  The  end-effector  and  the 
object  were  aligned  at  time  110  and  the  approach  state  was  activated.  At  time  117,  an  object 
was  detected  to  be  close  to  the  end-effector  (distance  model  result  fprox  evaluated  true).  Since 
object  diameters  were  less  than  the  diameter  modeling  set  points,  it  was  assumed  that  an  object 
was  partially  occluding  the  view  of  the  fruit.  The  occluded  state  was  activated  to  extend  the 
end-effector  through  the  obstruction  while  using  vision  control  on  joints  0  and  1  to  maintain  end- 
effector  alignment. 

While  the  occluded  state  was  active,  the  distance  to  the  object  increased  (fprox 
evaluated  false)  and  an  object  was  still  being  located  in  the  image  (fviser  evaluated  true).  This 
indicated  that  the  end-effector  had  extended  past  the  obstruction  located  in  front  of  the  fruit. 
For  this  reason,  the  approach  state  was  activated  at  time  121.  Again,  the  inability  to 
maintain  alignment  with  the  fruit  caused  sequencing  between  the  approach  and  slow_out 
states.  When  proximity  was  detected  at  time  148,  the  object  diameters  exceeded  the  diameter 
model  set  points  (fpickd  evaluated  true)  and  the  vision_touch  state  was  activated.  The 
visionjouch  state  moved  the  end-effector  to  the  picking  location  by  establishing  the  position 
set  point  for  joint  2  to  the  p2pick  result  parameter.  When  the  picking  position  was  reached,  the 
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detach  state  was  activated  to  stop  all  joint  motion  and  close  the  picking  device.  After  a  time 
interval  elapsed  for  the  picking  device  to  close,  the  retract  state  was  activated  to  withdraw 
the  end-effector  from  the  canopy.  When  joint  2  reached  its  home  location,  the  drop  state  was 
activated  to  open  the  picking  device.  After  the  picking  device  time  interval,  the  lip_up  state 
was  activated  to  close  the  picking  device.  After  the  picking  device  time  interval,  the 
lip_down  state  was  activated  to  open  the  picking  device.  The  lip_up  and  lip_down  sequence 
was  used  to  clear  obstacles  remaining  in  the  picking  device  after  fruit  removal.  Another  pick 
cycle  was  initiated  by  activation  of  the  pick_pull_in  state. 

Detection  of  excessive  fruit  movement 

Detection  of  excessive  fruit  motion  was  necessary  to  reduce  the  number  of  unsuccessful 
pick  cycles.  A  fruit  that  was  approximately  90  cm  in  front  of  the  robot,  had  lateral  motion,  and 
was  partially  occluded  by  leaves  was  picked  and  documented.  The  state  execution  sequence  of 
this  pick  cycle  is  shown  in  Figure  7.4  along  with  plots  of  joint  l's  velocity  and  joint  2's  position. 
The  data  for  this  cycle  began  with  the  slow_out  state  active.  Valid  object  detection  was  not 
maintained  and  the  wait_pick  state  was  activated.  The  approach — slow_out  cycling  was 
present  after  an  object  was  located  due  to  the  inability  to  maintain  end-effector  alignment  or 
fruit  motion.  During  the  slow_out  state  at  time  35,  a  result  from  the  velocity  model  indicated 
that  joint  0  or  l's  was  above  modeling  criterion.  The  wait_velocity  state  was  activated  to  halt 
outward  movement  of  the  end-effector  until  the  velocities  were  within  modeling  criteria  and 
end-effector  alignment  was  established.  The  velocity  plot  of  joint  1  shows  when  the  velocity  of 
joint  1  exceeded  the  velocity  model  criteria.  The  response  lag  of  joint  2's  position  to  the 
wait_velocity  state  is  marked  on  the  plot  of  joint  2's  position. 

After  the  magnitude  of  joint  0  and  1  's  velocity  were  below  modeling  criteria  and 
alignment  was  established,  the  approach  state  was  activated.  The  inability  to  maintain 
alignment  during  approach  to  the  fruit,  improperly  tuned  modeling  criteria,  or  fruit  motion 
caused  cycling  between  the  approach  state  and  the  slow_out  state.  Proximity  to  an  object  during 
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Figure  7.4  Pick  cycle  when  excessive  fruit  motion  was  detected. 


one  control  cycle  and  not  having  proximity  during  the  next  control  cycle  caused  much  cycling 
between  the  approach,  slow_out,  and  occluded  states.  When  proximity  was  detected  at  time 
92,  the  object  diameters  exceeded  diameter  modeling  set  points  and  the  vision_touch  state  was 
activated.  The  picking  cycle  was  completed  by  activation  of  the  detach,  retract,  drop,  lip_up, 
lip_down,  and  pick_pull_in  states. 

Detection  of  stuck  condition  on  joint  2 

Detection  of  the  stuck  condition  on  joint  2  was  important  to  reduce  damage  to  the  robot 
and  tree  structures.  An  attempt  to  pick  a  fruit  that  was  approximately  90  cm  in  front  of  the 
robot  and  was  mostly  occluded  by  leaves  was  performed.  The  state  execution  sequence  of  this 
pick  cycle  is  shown  in  Figure  7.5  along  with  plots  of  joint  2's  servo-valve  control  word  and  joint 
2's  position.  Data  recorded  for  this  pick  cycle  started  during  the  search  sequence.  While  the 
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up_right  state  was  active,  an  object  was  located  at  time  62  and  the  point  state  was  activated  to 
establish  alignment.  Alignment  was  established  and  the  approach  state  was  activated  at  time 
63  to  initiate  outward  movement  of  the  end-effector.  During  end-effector  outward  movement, 
the  slow_out  state  was  activated  due  to  the  detection  of  misalignment.  At  time  82,  the  object 
was  lost  from  view  and  the  wait_pick  state  was  activated  to  move  the  end-effector  closer  to 
the  canopy.  An  object  was  located  in  the  image  at  time  94  and  the  approach  state  was 
activated. 
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Figure  7.5  Pick  cycle  recognizing  stuck  condition  on  joint  2  when  approaching  fruit. 


The  cycling  between  the  approach,  slow_out,  occluded  states  occurred  again  due  to 
problems  previously  explained.  During  extension  of  the  end-effector,  the  picking  device  was 
pushed  against  tree  structures.  The  stuck  model  reported  that  joint  2's  control  word  had  been 
above  a  set  point  limit  for  25  control  cycles  at  time  196.  (Notice  that  monitoring  the  position  of 
joint  2  was  not  adequate  to  detect  the  stuck  condition  since  the  end-effector  was  still  moving 
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outward  during  detection  of  the  stuck  condition.)  The  occluded  state  was  deactivated  and  the 
abort_pick  state  was  activated  through  the  ap_stuck  alias.  The  abort_pick  state  retracted  the 
end-effector  from  the  canopy.  When  the  position  of  joint  2  was  close  to  its  home  location,  the 
pick_pull_in  state  was  activated. 

The  next  data  set  documents  the  recognition  of  joint  2's  stuck  condition  during  retraction 
from  the  canopy  (Figure  7.6)  and  includes  plots  of  joint  2's  position  and  servo-valve  control  word 
and  the  state  execution  sequence.  The  fruit  was  approximately  50  cm  in  front  of  the  robot  and 
partially  occluded  by  leaves.  The  state  execution  sequence  shows  that  a  previous  pick  cycle 
was  halted  by  the  abort_pick  state  and  the  search  sequence  initiated.  An  object  was  located 
during  the  downjeft  state  and  the  point  state  was  activated  to  align  the  end-effector  on  the 
fruit.  After  alignment  was  established,  the  approach  state  was  activated  at  time  20  to  extend 
the  end-effector  toward  the  object.  The  slow_out  state  was  activated  after  the  approach  state 
was  active  for  one  control  cycle  due  to  improperly  tuned  alignment  model  criteria.  When 
alignment  was  established  in  the  slow_out  state,  the  approach  state  was  activated.  When 
proximity  to  an  object  was  detected  at  time  68,  the  occluded  state  was  activated  due  to  object 
diameter  measurements  below  modeling  criteria.  During  the  occluded  state,  the  view  of  the 
object  was  lost  and  the  touch  state  was  activated. 

The  touch  state  used  dead  reckoning  to  move  the  end-effector  into  the  canopy.  When 
the  end-effector  was  extended  the  prescribed  distance,  the  detach  state  was  activated  to  close 
the  picking  device.  After  a  time  interval  elapsed  for  the  picking  device  to  close,  the  retract 
state  was  activated,  withdrawing  the  end-effector  from  the  canopy.  When  the  picking  device 
was  closed  to  capture  the  fruit  stem  between  the  picking  device  lip  and  the  end-effector 
housing,  a  tree  limb  was  also  captured.  During  the  retract  state,  the  stuck  condition  was 
reported  at  time  148.  The  release  state  was  activated  at  time  149  to  open  the  picking  device 
and  the  pick_pull_in  state  was  activated  after  the  picking  device  time  interval  had  elapsed. 
Joint  2  was  withdrawn  from  the  canopy  in  the  pick_pull_in  state  and  another  pick  cycle  was 
initiated  by  activation  of  the  fruit_check  state. 
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Figure  7.6  Pick  cycle  recognizing  stuck  condition  on  joint  2  when  retracting  joint  2. 

Detection  of  out-of-reach  fruit 

The  data  shown  in  Figure  7.7  presents  the  state  execution  sequence  and  the  position  of 
joint  2  for  a  pick  cycle  when  a  fruit  was  located  out  of  joint  2's  reach.  The  data  recorded  for  this 
pick  cycle  started  when  the  point  state  was  active.  When  alignment  between  the  end-effector 
and  the  object  was  achieved,  the  approach  state  was  activated.  The  approach  state  was 
active  for  one  control  cycle  due  to  improper  tuning  of  the  alignment  model  criteria.  Losing  sight 
of  the  object  caused  cycling  between  the  approach  (or  slow_out)  state  and  the  wait_pick  state. 
At  time  122,  proximity  to  an  object  was  detected  along  with  small  diameter  measurements  and 
the  occluded  state  was  activated.  At  time  124,  the  distance  to  the  object  increased  and  the 
approach  state  was  activated.  The  distance  to  an  object  decreased  below  the  distance  model 
criteria  at  time  125  along  with  small  object  diameters  and  the  occluded  state  was  activated.  At 
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time  127,  the  maximum  allowable  extension  of  joint  2  was  reached.  The  pick  cycle  was  aborted 
by  activation  of  the  abort_pick  state  through  the  ap_p2max  alias.  The  down_right  state  was 
activated  when  joint  2  reached  its  home  location  to  initiate  search  for  another  fruit. 
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Figure  7.7  Picking  cycle  documenting  detection  of  out-of-reach  fruit. 


Discussion  of  Picking  Intelligence 

From  the  plots  presented  in  this  chapter,  it  can  be  seen  that  the  picking  intelligence 
was  able  to  recognize  and  respond  to  various  situations  occurring  when  picking  fruit  grown  in 
actual  production  conditions.  The  search  technique  used  to  move  the  end-effector  through  the 
robot  work  space  coupled  with  forward  movement  of  the  trailer  provided  coverage  of  the 
canopy.  The  picking  intelligence  recognized  when  fruit  were  out  of  the  robot's  work  space  and 
aborted  the  pick  cycle.  Excessive  fruit  motion  was  detected  and  handled  to  reduce  the  number  of 
unsuccessful  pick  cycles.  The  procedure  used  to  obtain  a  closer  canopy  view  when  the  view  of  an 
object  was  lost  increased  the  number  of  successful  pick  cycles.  Recognition  of  partially  occluded 
fruit  increased  the  successful  removal  rate.  Collisions  between  the  robot  and  tree  were  detected 
by  monitoring  the  control  word  on  joint  2  and  avoided  robot  damage. 

The  cumulative  record  of  each  state's  active  time  was  used  to  document  how  long  each 
state  was  active  when  continuously  picking  fruit.  Three  picking  runs  were  made  while  the 
trailer  was  pulled  through  an  orange  grove.  The  total  time  that  the  robot  operated  during  the 
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three  runs  was  349  minutes.  Discounting  the  time  spent  in  the  power  up  and  power  down  state 
sequences,  the  time  spent  in  the  picking  and  search  sequences  was  81  minutes.  The  search  states 
(up_right,  upjeft,  down_right,  and  downjeft)  accounted  for  46  minutes  of  the  picking  time.  A 
total  of  35  minutes  were  spent  in  states  directly  associated  with  picking  fruit.  The  total  time 
spent  picking  fruit  included  the  time  spent  aborting  pick  cycles  for  various  reasons.  An 
estimated  321  completed  pick  cycles  (attempts  to  remove  fruit)  were  performed  during  the  35 
minutes  of  picking.  The  average  time  spent  during  one  pick  cycle  (including  the  time  spent  on 
aborted  attempts)  was  6.5  seconds.  An  average  of  15.1  seconds  per  pick  cycle  resulted  if  the  time 
spent  searching  for  fruit  was  included  in  the  average  calculation.  The  percentage  of  state  time 
spent  picking  based  on  the  35  minutes  of  picking  operation  are  shown  in  Table  7.1. 


Table  7.1  Percent  of  picking  time  spent  in  each  picking  sequence  state. 
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One  important  result  shown  in  this  table  is  the  amount  of  time  spent  when  end-effector 
alignment  was  not  maintained.  Approximately  16  percent  of  the  picking  time  was  spent  in  the 
slow_out  state  and  8  percent  was  spent  in  the  approach  state.  This  indicated  a  problem  with 
the  ability  to  maintain  end-effector  alignment  and  suggests  that  further  work  needed  to  be 
performed  on  the  vision  controllers,  robot  design,  or  modeling  logic. 

Approximately  12  percent  was  spent  in  the  wait_velocity  state  indicating  that 
excessive  fruit  movement  was  definitely  a  problem.  Over  4  percent  of  the  picking  time  was 
spent  aborting  pick  cycles  due  to  robot  work  space  limit  detection.  A  majority  of  this  time  was 
spent  retracting  the  robot  arm  due  to  fruit  being  beyond  the  reach  of  the  picking  device  (state 
alias  ap_p2max).  This  suggests  that  the  robot  work  space  needed  to  be  increased  to  provide  a 
longer  reach  into  the  canopy.  Removing  trash  from  the  end  effector  accounted  for  over  8  percent 
of  the  picking  time  (activation  of  the  lip_up  and  lip_down  states).  This  suggests  that  the 
picking  device  needed  to  be  improved  to  eliminate  the  time  spent  for  this  action. 

Several  problems  with  the  intelligence  to  pick  fruit  were  observed.  The  inability  to 
maintain  end-effector  alignment  with  an  object  caused  cycling  between  the  approach  and 
slow_out  states.  Examination  of  the  alignment  modeling  criteria  (Table  7.2)  showed  that 
additional  tuning  of  the  numerical  value  of  the  set  point  parameters  would  reduce  state  cycling. 
The  point  state  was  used  to  perform  the  initial  alignment  of  the  end-effector  to  an  object. 
Larger  values  for  the  set  points  were  programmed  in  the  initialization  section  for  the  point 
state.  Default  values  for  the  modeling  criteria  were  used  for  the  approach  and  slow_out  states. 
When  end-effector  alignment  was  established  in  the  point  state,  the  approach  state  was 
activated.  The  alignment  model  used  lower  numerical  criteria  values  in  the  approach  state. 
Thus,  after  alignment  was  established  in  the  point  state  and  the  approach  state  was  activated, 
the  slow_out  state  was  immediately  activated  due  to  misalignment.  The  slow_out  state  used 
the  same  numerical  values  for  the  centroid  error  model  criteria  as  the  approach  state.  When 
alignment  was  established  in  the  slow_out  state,  the  approach  state  was  activated.  Movement 
of  the  fruit  or  the  end-effector  would  result  in  misalignment  and  the  slow_out  state  was 
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reactivated.  The  modeling  criteria  for  these  three  states  needed  to  be  adjusted  to  better 
approximate  the  desired  actions.  The  numerical  value  for  the  alignment  modeling  criteria 
during  the  point  state  should  have  been  lower  so  the  end-effector  was  almost  perfectly  aligned 
with  an  object  before  starting  outward  movement.  The  modeling  criteria  used  during  the 
slow_out  state  should  have  been  larger  than  those  used  in  the  point  state  and  lower  than  those 
used  during  the  approach  state. 

Table  7.2  Alignment  model  criteria. 


State  name/model  criterion 

point 

approach 

slow_out 

viOslop 

70 

50 

50 

vilslop 

50 

35 

35 

Another  problem  with  the  intelligence  programmed  in  the  state  network  was  the 
cycling  between  the  approach  and  occluded  states.  The  cycling  between  these  states  involved 
the  sensitivity  of  the  ultrasonics  distance  readings.  The  switch  from  the  approach  state  to  the 
occluded  state  was  made  when  proximity  to  an  object  was  detected  and  object  diameters  did  not 
meet  the  diameter  model  criteria.  A  switch  from  the  occluded  state  to  the  approach  state  was 
made  when  the  distance  to  the  fruit  was  greater  than  the  distance  criteria  and  an  object  was 
still  being  located.  Thus,  a  slight  movement  of  the  fruit  away  from  the  picking  device 
increased  the  distance  reported  from  the  ultrasonics  transducer.  Also,  from  the  data  plot  of 
distance  versus  time  in  Figure  7.2,  it  can  be  observed  that  the  distance  information  should  have 
been  filtered  to  smooth  the  rapid  transitions  of  the  data  values.  Another  problem  associated 
with  the  ultrasonic  transducer  was  the  limited  effective  range  of  the  device  (between  9  and  40 
cm)  and  the  sensitivity  of  targeting  on  a  spherical  object. 

Picking  Language  Evaluation 
Evaluation  of  the  picking  language  involved  examining  the  ease  that  picking 
intelligence  was  specified,  modified,  and  debugged.  The  picking  language  was  designed  for 
programming  a  robot  to  perform  a  task  in  an  unstructured  environment  where  environmental 
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sensor  information  would  be  required  during  task  operation  to  alter  program  flow.  This  design 
criteria  fits  many  of  the  areas  that  robots  must  work.  Separation  of  input  and  output  software 
drivers  from  the  picking  language  added  to  the  language  flexibility  since  different  robot  tasks 
require  different  sensed  quantities  and  actuation. 

Input  from  sensors  and  output  to  actuation  devices  were  represented  in  the  picking 
language  in  data  base  structure  forms.  The  programmer  had  access  to  sensor  data  and  control  set 
points  and  could  specify  which  feedback  mode  was  utilized  for  actuator  driver  software.  Since 
input  and  output  hardware  and  driver  software  are  robot  specific,  the  segregation  of  input  and 
output  from  the  picking  language  provided  an  excellent  portability  factor.  For  example,  the 
complexity  of  output  drivers  software  determined  how  joint  control  was  established.  Position, 
velocity,  and  vision  feedback  were  defined  for  joint  control  for  the  robot  used  in  this  study.  The 
picking  language  adapted  to  the  complexity  of  joint  control  by  including  driver  information.  On 
another  robot,  output  drivers  may  also  include  force  control.  The  picking  language  would  adapt 
by  including  the  ability  to  alter  force  set  points  and  the  control  feedback  strategy.  More 
complex  output  drivers  could  be  developed  to  include  straight-line  and  coordinated  motion  for 
an  end-effector.  The  picking  language  would  adapt  to  the  complexity  of  joint  output  drivers  by 
providing  control  mode  selection. 

The  ability  to  specify  robot  actions  and  to  determine  when  events  occurred  in  the 
environment  were  programmed  in  states.  The  state  initialization  section  was  used  to  specify 
robot  actions  to  complete  during  a  state's  activation.  Models  were  programmed  to  monitor 
events  in  the  data  base  and  mapped  sensor  data  to  binary  results.  Modeling  criteria  could  be 
altered  for  an  individual  state  that  required  a  different  modeling  function  during  activation. 
Model  results  were  used  when  programming  state  links  instead  of  programming  modeling  logic 
directly  in  each  state.  This  simplified  programming  state  links  and  provided  a  uniformity 
among  state  links.  The  process  of  adding  intelligence  was  enhanced  since  results  from  existing, 
working  models  could  be  used  when  programming  new  state  links. 
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Decision  making  capabilities  were  available  at  a  local  state  level  and  at  a  global 
level.  Global  decision  making  provided  the  ability  to  detect  system  wide  errors.  By  using  the 
safety  links,  a  programmer  did  not  have  to  include  detection  of  system  errors  in  each  state 
added  to  the  state  network.  The  decision  making  at  a  local  state  level  provided  the  capability 
to  detect  when  robot  actions  had  been  completed  or  problems  existed. 

Each  robot  action  specification  (a  state)  was  assigned  a  name  by  the  programmer.  By 
using  a  state  name  that  was  indicative  of  the  state  function,  debugging  the  program  was 
enhanced.  Modifications  to  the  program  were  made  by  adding  new  states  and  models.  To 
include  a  new  state  in  the  state  network,  a  state  link  from  an  existing  state  was  programmed. 
Links  from  the  new  state  were  made  to  existing  states  in  the  network  (or  to  newly  added  states). 
Alterations  to  the  whole  program  did  not  have  to  be  made  to  recognize  the  new  links. 

Parameter  definition  was  also  a  component  of  the  picking  language  and  provided  the 
capacity  to  define  data  in  a  data  base  storage  area.  Assignment  parameters  were  used  to  reduce 
debugging  time  and  provided  the  capacity  to  alter  set  points  on-line  through  a  user  interface. 
Assignment  parameters  provided  a  quick  method  to  tune  modeling  criteria  set  points  since 
different  modeling  criteria  could  be  evaluated  on-line  without  the  need  to  recompile  the 
program.  System  parameters  were  used  for  communication  between  states  and  between  the  robot 
program  and  the  user.  Use  of  system  parameters  allowed  the  user  to  direct  robot  activity  and 
provided  a  method  of  sending  data  between  different  sections  of  the  robot  program.  System 
information  could  be  exchanged  between  states  by  altering  system  parameters  in  the  data  base. 

The  use  of  the  picking  language  greatly  facilitated  creation  and  modification  of  the 
intelligence  to  pick  grove  fruit.  Because  of  the  modularity  of  the  language,  solutions  to 
problems  that  were  observed  during  grove  operation  could  be  quickly  added  to  the  existing 
program.  Since  the  program  could  be  altered  quickly,  more  time  was  spent  observing  and  solving 
additional  problems  encountered  during  operation.  The  ability  to  give  functional  names  to 
states  facilitated  debugging  the  state  network.  The  use  of  state  alias  names  enabled 
programmers  to  determine  the  reason  a  state  was  activated  during  run-time  instead  of  observing 
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logged  data.  This  enhanced  on-line  debugging  because  of  the  considerable  time  factor  involved 
in  printing  data  to  a  console  screen  and  plotting  data. 


CHAPTER  8 
SUMMARY  AND  CONCLUSIONS 


Summary 

A  programming  language  was  developed  to  provide  a  systematic  and  modular  approach 
for  specifying  intelligence  for  a  vision-servoed  fruit-picking  robot.  The  language  was  used  to 
develop  a  task-level  control  program  for  a  fruit-picking  robot.  The  robot  consisted  of  a 
manipulator,  computer  hardware  and  software,  and  a  support  trailer.  The  manipulator  was  a 
servo-hydraulic,  spherical-coordinate  robot  with  a  sensor  package  for  end-of-the-arm  fruit 
sensing.  Input  drivers  were  used  to  input  information  from  sensors  and  output  drivers  were  used 
to  calculate  and  output  information  to  actuation  devices. 

The  picking  language  consisted  of  models,  states,  and  a  real-time  data  base.  Models 
mapped  sensor  and  system  information  to  binary  results  according  to  criteria  established  by  a 
programmer.  States  specified  robot  actions  and  used  model  results  to  determine  when  actions 
had  been  completed  or  if  problems  existed  that  prevented  completion  of  specified  actions.  A 
state  network  was  created  by  linking  states  together  with  decision  statements. 

A  state  has  two  fundamental  components — action  specification  and  decision  making. 
Actions  were  specified  by  modifying  feedback  control  flags,  control  set  points,  and  modeling 
criteria  stored  in  the  data  base.  Decision  making  was  programmed  in  state  links  which  joined 
states  together  in  a  network.  State  links  evaluated  the  status  of  modeling  results  and  system 
data  and  indicated  when  and  which  state  in  the  network  was  activated.  Alterations  to  an 
existing  network  were  made  by  adding  new  states  and  altering  links  of  existing  states. 

A  structured  interface  between  the  picking  language  and  input  and  output  drivers 
provided  access  to  sensor  information  and  specification  of  actuator  actions.  The  picking 
language  provided  the  capability  to  define  parameters  in  the  data  base  that  were  used  for 
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communication  and  on-line  debugging  purposes.  Assignment  parameters  established  numerical 
values  for  control  set  points  and  modeling  criteria  and  provided  on-line  adjustment  capability. 
System  parameters  provided  communication  between  a  user  interface  and  the  intelligence 
program  and  storage  of  data  between  states. 

The  developed  state  network  provided  the  vision-servoed  robot  with  the  intelligence 
needed  to  automatically  pick  fruit.  The  picking  intelligence  recognized  and  solved  many 
problems  associated  with  grove  operation  of  the  robot.  Grove-related  problems  that  were 
addressed  included  attempting  to  pick  fruit  that  were  out  of  the  robot's  work  space;  detecting 
excessive  fruit  motion  that  prevented  a  successful  pick  cycle;  detecting  when  a  fruit  vanished 
from  view;  picking  fruit  that  were  partially  occluded  by  limbs  and  leaves;  and  detecting 
collisions  between  the  robot  and  tree.  Over  150  hours  of  development  time  were  spent  with  the 
robot  in  grove  settings.  Fruit  were  successfully  picked  under  a  variety  of  production  conditions. 
During  a  35-minute  period  of  fruit  picking,  321  complete  pick  cycles  were  performed.  This 
averaged  to  approximately  6.5  seconds  per  pick  cycle.  It  was  estimated  that  75%  of  the  fruit 
that  the  robot  attempted  to  pick  were  actually  removed. 

Conclusions 

Two  specific  objectives  of  this  work  were  to  define  a  programming  language  for  a  fruit- 
picking  robot  and  develop  a  program  to  pick  fruit  grown  in  production  conditions.  Both  of  these 
objectives  were  met.  The  following  conclusions  about  the  picking  language  and  the  picking 
program  were  drawn. 

Conclusion  1 

The  language  developed  in  this  work  was  well  suited  for  development  of  a  program  to 
control  a  robot  operated  in  an  unstructured  environment.  The  picking  language  provided  a 
consistent  framework  for  creating,  modifying,  and  debugging  a  program.  The  consistent 
interface  between  the  language  and  input  and  output  drivers  provided  the  flexibility  to  port 
the  language  to  other  robotic  systems.  The  modularity  of  the  picking  language  enabled  problem 
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solutions  to  be  quickly  added  to  the  program  without  major  modifications  to  the  remaining 
program. 

Definition  of  models  provided  a  method  to  assimilate  environmental  and  system 
information.  Use  of  model  results  in  decision  making  provided  a  consistent  method  to  program 
new  state  links.  When  additional  intelligence  was  programmed  to  solve  a  problem,  results  from 
existing,  working  models  could  be  used  in  state  links.  If  the  modeling  logic  was  later  changed, 
the  logic  only  had  to  be  changed  in  one  location;  every  state  link  that  used  the  modeling  logic 
did  not  have  to  be  altered.  Assignment  parameters  were  used  to  establish  numerical  values  for 
modeling  criteria  during  state  initialization  and  were  alterable  on-line  through  the  user 
interface.  This  decreased  the  amount  of  time  required  to  tune  modeling  criteria. 

Conclusion  2 

Additional  features  should  be  incorporated  into  the  picking  language  to  extend  its 
functionality.  One  feature  deals  with  the  concurrent  execution  of  several  state  networks  for 
control  of  a  multi-arm  fruit  harvester.  Each  robot  in  the  harvester  may  have  a  local  system 
(consisting  of  a  manipulator,  scheduler,  input  and  output  drivers,  models,  and  state  network) 
and  a  data  base  in  common  with  the  other  robot  systems.  A  supervisory  state  network  would 
monitor  the  common  data  base  to  perform  collision  avoidance,  command  one  robot  to  stop 
picking,  or  modify  model  criteria  for  one  controller  system  to  improve  picking  performance. 

Another  feature  would  be  incorporation  of  adaptability  for  model  criteria.  The  use  of 
models  was  an  important  language  feature  due  to  the  ability  to  assimilate  environmental 
information  into  meaningful  results.  When  actually  programming  models,  values  for  modeling 
criteria  had  to  be  estimated  and  then  fine  tuned  during  robot  operation.  One  reason  assignment 
parameters  were  a  component  of  the  picking  language  was  the  desire  to  alter  modeling  criteria 
on-line  to  speed  determination  of  tuned  values  for  model  criteria.  An  additional  feature  of  the 
language  would  be  the  capability  to  adapt  model  criteria  automatically  to  determine  optimal 
values.  Provisions  would  be  required  to  establish  how  the  criterion  was  varied  (specifying 


118 

range,  standard  deviation,  and  variance)  as  well  as  a  method  to  determine  the  adequacy  of  the 
adapted  criterion. 

If  one  state  was  designated  as  a  starting  location  and  another  state  as  a  goal,  a  measure 
of  the  adequacy  of  the  criteria  would  be  the  ability  to  reach  the  goal  and  awards  given  to 
modeling  criteria  if  the  goal  was  reached.  Other  factors;  least  number  of  states  activated, 
least  amount  of  robot  energy  expended,  or  least  amount  of  time  required  to  reach  the  goal;  could 
be  included  in  the  award  system.  Statistics  could  be  compiled  to  determine  when  criteria  were 
at  their  optimal  value. 

Conclusion  3 

The  program  developed  for  the  vision-servoed,  fruit-picking  robot  successfully 
addressed  five  common  problems  encountered  during  grove  operation.  These  problems  were 
identified  as  picking  fruit  that  were  outside  the  robot's  work  space,  picking  fruit  with 
excessive  motion,  losing  sight  of  a  fruit  during  a  pick  cycle,  picking  partially  occluded  fruit,  and 
collisions  between  the  end-effector  and  tree  structures. 

Detection  of  a  work  space  violation  was  handled  by  aborting  the  pick  cycle,  linearly 
withdrawing  the  end-effector  from  the  canopy,  and  initiating  a  search  for  another  fruit. 
Excessive  fruit  motion  was  effectively  handled  by  programming  two  solutions.  At  the  first 
indication  of  fruit  motion,  outward  velocity  of  the  end-effector  was  reduced.  In  extreme  cases  of 
fruit  motion,  the  outward  velocity  of  the  end-effector  was  stopped  until  fruit  motion  was 
within  an  acceptable  picking  tolerance.  Loss  of  fruit  detection  was  handled  with  two  solutions. 
When  a  fruit  was  lost  from  view,  the  end-effector  was  slowly  moved  outward  to  obtain  a  closer 
view  of  the  canopy.  If  a  fruit  was  not  located  while  the  end-effector  moved  outward  for  a  short 
distance,  the  pick  cycle  was  aborted.  The  use  of  object  size  was  adequate  to  detect  when  a  single 
fruit  was  partially  occluded.  This  method  of  occlusion  detection  was  not  adequate  when 
attempts  were  made  to  pick  fruit  in  clusters. 
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Collisions  between  the  robot  and  a  tree  structure  were  successfully  detected  by 
monitoring  the  servo-valve  command  to  joint  2  to  indicate  when  joint  2  was  stuck.  When  the 
stuck  condition  was  detected  during  extension  of  the  end-effector  into  the  canopy,  the  pick  cycle 
was  aborted.  When  the  stuck  condition  was  detected  during  retraction  of  the  end-effector  from 
the  canopy,  one  of  two  situations  was  present.  Either  the  picking  device  was  grasping  a  tree 
structure  that  was  not  detachable  by  the  picking  device  or  another  part  of  the  picking  device 
was  caught  on  a  tree  structure.  The  only  solution  programmed  for  a  stuck  condition  when 
retracting  the  end-effector  was  to  open  the  picking  device  to  release  the  tree  structure  and  then 
abort  the  pick  cycle.  A  solution  to  the  other  situation  was  not  programmed  but  should  be 
addressed. 

During  execution  of  the  picking  program,  a  rapid  cycling  between  several  states  was 
observed.  The  cyclic  nature  between  the  approach  and  slow_out  states  was  recognized  as 
inadequate  specification  of  modeling  criteria,  motion  of  fruit,  and  inadequate  vision 
compensators.  Another  cyclic  problem  was  observed  between  the  approach  (or  slow_out)  state 
and  the  occluded  state  and  was  recognized  as  inadequately  processed  distance  information  and 
inadequately  tuned  distance  modeling  criterion.  It  was  concluded  that  these  problems  should  be 
addressed  to  improve  picking  performance.  Detection  of  whether  a  fruit  was  actually  removed 
during  a  pick  cycle  was  another  improvement  to  the  picking  intelligence  that  should  be 
implemented.  Information  from  successful  and  unsuccessful  pick  cycles  could  be  examined  to 
determine  changes  needed  to  improve  the  success  rate  of  removing  fruit. 

Conclusion  4 

To  take  full  advantage  of  the  picking  language,  a  programming  environment  must  be 
developed.  A  text  editor  and  a  C  programming  language  compiler  were  used  to  generate  and 
compile  the  robot  program,  user  interface,  data  logger,  scheduler,  data  base,  and  models  for  this 
research.  A  large  amount  of  time  was  spent  coding  each  of  these  components  when  making 
alterations  to  the  picking  program.  A  programming  environment  that  included  editing 
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facilities  and  a  compiler  for  the  picking  language  would  have  greatly  facilitated  the  process  of 
creating  and  modifying  the  picking  program. 

Editing  facilities  to  create  text  files  for  a  state  network  map,  models,  system  and 
assignment  parameters,  and  macros  would  be  required.  Features  of  editing  a  state  network  map 
would  include  the  capability  to  define  a  new  state,  remove  a  state,  and  prioritize  the  state 
links.  A  compiler  for  the  picking  language  would  read  text  files  and  input  and  output  data  base 
structures  to  generate  the  global  data  base,  the  scheduler,  the  data  logger,  the  user  interface, 
and  the  robot  program.  This  could  be  accomplished  by  first  producing  C  source  files  and  using  a 
conventional  C  compiler  to  complete  the  process. 

The  complexity  of  the  editing  facilities  and  compiler  are  realized  when  an  example  of 
adding  a  new  model  is  examined.  Besides  the  text  editing  features  required  to  actually  program 
the  model,  the  state  network  map  had  to  be  updated  to  include  the  modeling  criteria  in  the 
state  initialization  section.  The  data  logger  had  to  be  updated  if  the  model  result  was  marked 
for  real-time  logging.  The  user  interface  had  to  be  updated  to  provide  the  capacity  to  alter  and 
display  the  model  criteria  on-line.  Data  base  storage  had  to  be  reserved  for  the  model  result 
and  criteria. 


APPENDIX  A 


DATA  BASE  STRUCTURES  FOR  INPUT  AND  OUTPUT  DRIVERS, 
MACROS,  AND  SYSTEM  AND  ASSIGNMENT  PARAMETERS 


Macro  Substitutions 

Name 

Replacement  String 

Comment 

TRUE 

1 

Boolean  true  value 

FALSE 

0 

Boolean  false  value 

ABS(x) 

((x)  <  0.0)  ?  (-(x))  :  (x) 

returns  absolute  value  of  expression  x 

ON 

1 

end-effector  lights  on 

OFF 

0 

end-effector  lights  off,  picking  device  off 

OPEN 

1 

picking  device  open 

CLOSED 

2 

picking  device  closed 

P 

0 

position  control  law  calculations,  joints  0, 1,  and  2 

V 

1 

velocity  control  law  calculations,  joints  0, 1,  and  2 

J 

2 

joy  stick  control  law  calculations,  joints  0, 1,  and  2 

VI 

3 

vision  control  law  calculations,  joints  0  and  1 

Figure  A.l  Macro  substitutions  defined  in  input  and  output  drivers. 


System  Parameters 

Name 

Type 

Range 

Initial 

Log 

Comment 

fdata 

flag 

ON  II  OFF 

OFF 

activation  flag  for  data  logger  routine 

state_time 

long  int 

0 

time  count  for  state  activation 

Figure  A.2  System  parameters  supplied  to  programming  language. 


Assignment  Parameters 

Name 

Assign  to 
Parameter 

Range 

Initial 

Log 

Comment 

pOhi 

pOsp 

36.2 

c 

joint  0  high  position  (deg) 

pOlo 

pOsp 

-36.2 

c 

joint  0  low  position  (deg) 

pOmid 

pOsp 

0.0 

c 

middle  position  for  joint  0  (deg) 

plhi 

pOsp 

19.2 

c 

joint  1  high  position  (deg) 

pllo 

plsp 

-19.2 

c 

joint  1  low  position  (deg) 

pi  mid 

plsp 

0.0 

c 

middle  position  for  joint  1  (deg) 

p2hi 

p2sp 

137.7 

c 

joint  2  high  position  (cm) 

p21o 

p2sp 

2.0 

c 

joint  2  low  position  (cm) 

p2mid 

p2sp 

54.5 

c 

middle  position  for  joint  2  (cm) 

vOmax 

vOsp 

162.8 

c 

joint  0  maximum  velocity  (deg/s) 

vOmin 

vOsp 

-162.8 

c 

joint  0  minimum  velocity  (deg/s) 

vlmax 

vlsp 

162.8 

c 

joint  1  maximum  velocity  (deg/ s) 

vlmin 

vlsp 

-162.8 

c 

joint  1  minimum  velocity  (deg/s) 

v2max 

v2sp 

139.2 

c 

joint  2  maximum  velocity  (cm/s) 

v2min 

v2sp 

-139.2 

c 

joint  2  minimum  velocity  (cm/s) 

viOmid 

viOsp 

242 

c 

mid  value  for  vision  y  (pixels) 

vilmid 

vilsp 

192 

c 

mid  value  for  vision  x  (pixels) 

Figure  A.3  Assignment  parameters  defined  in  input  and  output  drivers. 
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Name:     Joint  position  input  driver. 

Function:  Digitize  each  joint  potentiometer  voltage  level  and  convert  to  joint  position. 

Name 

Field 

Type 

Range 

Log 

Comment 

pO 

result 

double 

<  39.0  &&  >  -39.0 

1 

position  of  joint  0  (degrees) 

P1 

result 

double 

<  23.0  &&> -23.0 

1 

position  of  joint  1  (degrees) 

P2 

result 

double 

<  130.0  &&>1.0 

1 

position  of  joint  2  (centimeters) 

Figure  A.4  Data  base  structure  for  joint  position  input  driver  software. 


Name:     Joint  vel< 
Function:  Digitize 

xity  input  driver. 

oint  tachometer  voltage  levels  and  convert  to  a  joint  velocity. 

Name 

Field 

Type 

Range 

Log 

Comment 

vO 

result 

double 

<  165.0  &&  >  -165.0 

1 

velocity  of  joint  0  (deg/s) 

vl 

result 

double 

<  165.0  &&  >  -165.0 

1 

velocity  of  joint  1  (deg/s) 

v2 

result 

double 

<  140.0  &&  >  -140.0 

1 

velocity  of  joint  2  (cm/s) 

Figure  A.5  Data  base  structure  for  joint  velocity  input  driver  software. 


Name:     Joy  stick  position  input  driver. 

Function:  Digitize  joy  stick  potentiometer  voltage  levels  and  convert  to  a  position  offset. 

Name 

Field 

Type 

Range 

Log 

Comment 

jpO 

result 

int 

<2048  &&>-2048 

joy  stick  0  position  offset  from  null  (D/A  units) 

result 

int 

<  2048  &&> -2048 

joy  stick  1  position  offset  from  null  (D/A  units) 

)P2 

result 

int 

<  2048  &&  >  -2048 

joy  stick  2  position  offset  from  null  (D/A  units) 

Figure  A.6  Data  base  structure  for  joy  stick  position  input  driver  software. 


Name: 

Distance  input  driver. 

Function:  Read  count  value  from  a  counter/timer  hardware  and  convert  to  a  distance  reading. 

This  value  is  added  to  the  position  of  joint  2  to  obtain  a  picking  position. 

Name 

Field 

Type 

Range 

Log 

Comment 

distp2 

result 

double 

<  100.0  &&  >  0.0 

1 

distance  to  fruit  (cm) 

p2pick 

result 

double 

<  130.0  &&>1.0 

joint  2's  position  to  pick  fruit  (cm) 

Figure  A.7  Data  base  structure  for  distance  input  driver  software. 


Name:     Power  input  driver. 

Function:  Obtain  the  status  of  power  to  the  HPU  dump  valve  solenoid,  interior  push  buttons, 
servo-amplifier,  and  both  joy  stick  push  buttons.  Power  to  device  =  TRUE. 

Name 

Field 

Type 

Range 

Log 

Comment 

psdmp 

result 

flag 

TRUE  II  FALSE 

dump  valve  solenoid  power  status 

pssapr 

result 

flag 

TRUE  II  FALSE 

servo-amplifier  power  status 

pspbut 

result 

flag 

TRUE  II  FALSE 

interior  push  buttons  power  status 

psjbutO 

result 

flag 

TRUE  II  FALSE 

status  of  joy  stick  push  button  0 

psjbutl 

result 

flag 

TRUE  II  FALSE 

status  of  joy  stick  push  button  1 

Figure  A.8  Data  base  structure  for  power  input  driver  software. 
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Name:     Vision  input  driver. 

Function:  Obtain  the  centroids  and  diameters  of  a  foreground  colored  object  located  in  an 

image  and  determine  the  average  intensity  of  foreground  pixels.  If  a  valid  object 
can  not  be  located,  the  average  intensity  of  background  pixels  is  determined. 


Name 

Field 

Type 

Range 

Log 

Comment 

fblob 

result 

flag 

TRUE  II  FALSE 

result  of  object  quantification 

xcent 

result 

long  int 

<  385  &&  >=  0 

horizontal  centroid  location  of  object  (pixels) 

xdia 

result 

long  int 

<  385  &&  >=  0 

horizontal  diameter  of  object  (pixels) 

ycent 

result 

long  int 

<  485  &&  >=  0 

vertical  centroid  location  of  object  (pixels) 

ydia 

result 

long  int 

<  485  &&  >=  0 

vertical  diameter  of  object  (pixels) 

iavef 

result 

int 

<=32&&>=0 

average  intensity  of  fruit  pixels 

iaveb 

result 

int 

<=32  &&>=0 

average  intensity  of  background  pixels 

Figure  A.9  Data  base  structure  for  vision  input  driver  software. 


Name:     Joint  0  output  driver. 

Function:  Calculate  joint  output  value  using  position,  velocity,  vision,  or  joy  stick  feedback 
control  and  output  servo-valve  control  word  to  D/A  converter  connected  to  joint  0 
servo-actuator.  Macros  defined  for  joint  feedback  control  activation: 

P  =  position,  V  =  velocity,  J  =  joy  stick,  VI  =  vision. 

Name 

Field 

Type 

Range 

Log 

Comment 

vOcw 

result 

int 

<  2048  &&  >  -2048 

joint  0  servo-valve  control  word 

jointOctl 

activation 

int 

P II V  II J  II VI 

joint  0  control  type  activation  flag 

pOsp 

set  point 

double 

<  pOhi  &&  >  pOlo 

joint  0  position  set  point  (deg) 

vOsp 

set  point 

double 

<  vOmax  &&  >  vOmin 

joint  0  velocity  set  point  (deg/s) 

viOsp 

set  point 

long  int 

>0&&<485 

joint  0  centroid  set  point  (pixels) 

Figure  A.10  Data  base  structure  for  joint  0  output  driver  software. 


Name:     Joint  1  output  driver. 

Function:  Calculate  joint  output  value  using  position,  velocity,  vision,  or  joy  stick  feedback 
control  and  output  servo-valve  control  word  to  D/A  converter  connected  to  joint  1 
servo-actuator.  Macros  defined  for  joint  feedback  control  activation: 

P  =  position,  V  =  velocity,  J  =  joy  stick,  VI  =  vision. 

Name 

Field 

Type 

Range 

Log 

Comment 

view 

result 

int 

<  2048  &&  >  -2048 

joint  1  servo-valve  control  word 

jointlctl 

activation 

int 

P II V  II J  II VI 

joint  1  control  type  activation  flag 

plsp 

set  point 

double 

<  plhi  &&  >  pllo 

joint  1  position  set  point  (deg) 

vlsp 

set  point 

double 

<  vlmax  &&  >  vlmin 

joint  1  velocity  set  point  (deg/s) 

vilsp 

set  point 

long  int 

>  0  &&  <  385 

joint  1  centroid  set  point  (pixels) 

Figure  A.  11  Data  base  structure  for  joint  1  output  driver  software. 
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Name:     Joint  2  output  driver. 

Function:  Calculate  joint  output  value  using  position,  velocity,  or  joy  stick  feedback  control 
and  output  servo-valve  control  word  to  D/ A  converter  connected  to  joint  2  servo- 
actuator.  Macros  defined  for  joint  feedback  control  activation: 


P  =  position,  V  =  velocity,  J  =  joy  stick. 


Name 

Field 

Type 

Range 

Log 

Comment 

v2cw 

result 

int 

<  2048  &&  >  -2048 

1 

joint  2  servo-valve  control  word 

joint2ctl 

activation 

int 

PIIVIIJ 

1 

joint  2  control  type  activation  flag 

p2sp 

set  point 

double 

<  p2hi  &&  >  p21o 

1 

joint  2  position  set  point  (cm) 

v2sp 

set  point 

double 

<  v2max  &&  >  v2min 

1 

joint  2  velocity  set  point  (cm/s) 

Figure  A.12  Data  base  structure  for  joint  2  output  driver  software. 

Name: 

Aperture  output  driver. 

Function:  Calculate  the  aperture  control  word  using  average  intensity  feedback  and  output 

control  word  to  red  image  board. 

Name 

Field 

Type 

Range 

Log 

Comment 

aptcw 

result 

int 

<=  31  &&  >=  0 

aperture  control  word 

Figure  A.  13  Data  base  structure  for  aperture  output  driver  software. 


Name:     Power  output  driver. 

Function:  Route  hydraulic  fluid  to  robot  if  fhydpwr  =  ON,  route  hydraulic  fluid  to  reservoir 
if  fhydpwr  =  OFF.  Remove  power  to  both  picking  device  solenoids  if  pdevice  = 
OFF,  activate  the  solenoid  that  moves  the  picking  device  to  the  open  (down) 
location  if  pdevice  =  OPEN,  activate  the  solenoid  that  moves  the  picking  device  to 
the  closed  (up)  location  if  pdevice  =  CLOSED.  Supply  power  to  end-effector  lights 
if  flights  =  ON,  remove  power  to  end-effector  lights  if  flights  =  OFF. 

Name       Field  Type  Range  Log  Comment 

fhydpwr  activation  flag  ON  II  OFF  power  to  external  push  button 

pdevice  activation  int     OFF  II  OPEN  II  CLOSED         picking  device  actuator  power 

flights   activation  flag  ON  II  OFF   end-effector  lights  power 


Figure  A.  14  Data  base  structure  for  power  output  to  external  push  button,  picking  device,  and 
end-effector  lights. 


APPENDIX  B 


DATA  BASE  STRUCTURES  FOR  ASSIGNMENT  AND 
SYSTEM  PARAMETERS  AND  MACROS  FOR  PICKPNG  PROGRAM 


Macro  Substitutions 

Name 

Replacement  String 

Comment 

UP 

1 

joint  0  direction  indicator  for  search  pattern 

DOWN 

0 

joint  0  direction  indicator  for  search  pattern 

RIGHT 

1 

joint  1  direction  indicator  for  search  pattern 

LEFT 

0 

joint  1  direction  indicator  for  search  pattern 

Figure  B.l  Macro  substitution  definitions. 


System  Parameters 

Name 

Type 

Range 

Initial 

Log 

Comment 

fpowerup 

flag 

ON  II  OFF 

OFF 

hydraulic  power  request  flag 

fpick 

flag 

TRUE  II  FALSE 

FALSE 

picking  sequence  flag 

ftrack 

flag 

TRUE  II  FALSE 

FALSE 

tracking  control  mode  flag 

fjoy 

flag 

TRUE  II  FALSE 

FALSE 

joy  stick  control  mode  flag 

fdirO 

flag 

UP  II  DOWN 

UP 

joint  0  search  direction  flag 

fdirl 

flag 

RIGHT  II  LEFT 

RIGHT 

joint  1  search  direction  flag 

wait 

long  int 

>=0 

60 

c 

search  delay  time  value  (1  /60  s) 

occdist 

double 

>=0.0 

21.9 

c 

occluded  distance  to  move  out  (cm) 

Figure  B.2  System  parameter  definitions. 


Assignment  Parameters 


Name 

Assign  to 
Parameter 

Range 

Initial 

Log 

Comment 

pOdn 

pOsp 

<  pOup  &&  >  pOmin 

-28.4 

c 

lower  search  position  (deg) 

pOhome 

pOsp 

<  pOmax  &&  >  pOmin 

0.0 

c 

home  position  for  joint  0  (deg) 

pOup 

pOsp 

<  pOmax  &&  >  pOdn 

23.3 

c 

upper  search  position  (deg) 

plhome 

plsp 

<  plmax  &&  >  plmin 

0.0 

c 

home  position  for  joint  1  (deg) 

pllf 

plsp 

<  plrt  &&  >  plmin 

-8.2 

c 

left  search  position  (deg) 

plrt 

plsp 

<  plmax  &&  >  pllf 

-0.9 

c 

right  search  position  (deg) 

p2home 

p2sp 

<  p2max  &&  >  p2min 

54.5 

c 

home  position  for  joint  2  (cm) 

pmdelay 

dttime 

>=0 

16 

c 

picking  device  time  delay  (1/60  s) 

vOdn 

vOsp 

<  0.0  &&  >  vOmin 

-16.3 

c 

joint  0  downward  velocity  (deg/s) 

vOup 

vOsp 

<  vOmax  &&  >  0.0 

16.3 

c 

joint  0  upward  velocity  (deg/s) 

vllf 

vlsp 

<  0.0  &&  >  vlmin 

-8.1 

c 

joint  1  left  velocity  (deg/s) 

vlrt 

vlsp 

<  vlmax  &&  >  0.0 

8.1 

c 

joint  1  right  velocity  (deg/s) 

v2fast 

v2sp 

<  v2max  &&  >  0.0 

55.7 

c 

fast  outward  velocity  (cm/s) 

v2medium 

v2sp 

<  v2max  &&  >  0.0 

45.7 

c 

middle  outward  velocity  (cm/s) 

v2ret 

v2sp 

<  0.0  &&  >  v2min 

-83.5 

c 

joint  2  return  velocity  (cm/s) 

v2slow 

v2sp 

<  v2max  &&  >  0.0 

35.7 

c 

slow  outward  velocity  (cm/s) 

viOppd 

viOpickd 

<  485  &&  >  0 

340 

c 

picking  y  diameter  (pixels) 

vilppd 

vilpickd 

<  385  &&  >  0 

210 

c 

picking  x  diameter  (pixels) 

waitdef 

wait 

>=0 

60 

c 

search  delay  value  (1/60  s) 

Figure  B.3  Assignable  parameter  definitions. 
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APPENDIX  C 
SAFETY  LINKS 


Safety  Links 

LlnK  State 

LlnK  LOglC 

V^UITUTieiU 

dump_pull_in 

1.  ipsdmp  &&  fhydpwr 

no  power  sensed  to  HPU  dump  valve  and  power 

ciir\nli£v?  1 1~\  ovf^ffial  mien  riii4"fr\Ti  ( nv tnrinr  T"incn 
SuLJDlltXl  IU  CAlcIIlal               UUllUIl  \t-Xlt:I  H>I  I'loll 

button  is  open). 

Humr)  null  in 

2  fhvdDwr  &&  '  fDoweruD 

power  sensed  to  HPU  dump  valve  and  power 
supplied  to  external  push  button  and  user 
requesting  robot  power  to  be  removed 

dump_pull_in 

3.  Ipssapr 

no  power  sensed  to  servo-amplifier 

dump_pull_in 

4.  Ipspbut 

no  power  sensed  to  console  push  buttons 

dump_pull_in 

5.  IpsjbutO  II  Ipsjbutl 

a  joy  stick  push  button  is  pressed 

dump_pull_in 

6.  (pO  >  pOhi)  II  (pO  <  pOlo) 

joint  O's  position  is  above  or  below  position  limits 

dump_pull_in 

7.  (pi  >  plhi)  II  (pi  <  pllo) 

joint  l's  position  is  above  or  below  position  limits 

dump_pull_in 

8.  (p2  >  p2hi)  II  (p2  <  p21o) 

joint  2's  position  is  above  or  below  position  limits 
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APPENDIX  D 
MODELS 


Model  Name:  Timer 

Function:        Determined  when  duration  of  state  active  exceeded  a  specified  time  limit.  Set 
fdt  to  true  if  state  active  time  (state_time)  was  greater  than  desired  state 
active  time  (dttime),  otherwise  set  fdt  to  false. 

Name 

Field 

Type 

Range 

Initial 

Log 

Comment 

fdt 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

result  flag 

dttime 

set  point 

long  int 

>=0 

0 

state  activate  time  (1/60  s) 

Logic: 

if(  dttime  <  state_time) 
else 

fdt  = 
fdt  = 

=  TRUE; 
=  FALSE; 

Figure  D.l  State  timer  model. 


Model  Name:  Joints  0, 1 ,  and  2  position  error 

Function:        Determined  when  joint  O's  position  error  magnitude  was  less  than  allowable 
limit.  Set  fpOslop  to  true  if  position  of  joint  0  (pO)  was  between  ±  position 
tolerance  (pOslop)  of  position  set  point  (pOsp),  otherwise  set  fpOslop  to  false. 

Determined  when  joint  l's  position  error  magnitude  was  less  than  allowable 
limit.  Set  fplslop  to  true  if  position  of  joint  1  (pi)  was  between  ±  position 
tolerance  (plslop)  of  position  set  point  (plsp),  otherwise  set  fplslop  to  false. 

Determined  when  joint  2's  position  error  magnitude  was  less  than  allowable 
limit.  Set  fp2slop  to  true  if  position  of  joint  2  (p2)  was  between  ±  position 
tolerance  (p2slop)  of  position  set  point  (p2sp),  otherwise  set  fp2slop  to  false. 


Determined  when  all  three  joints  were  within  position  limits.  Set  fpslop  to 
true  if  all  joint  positions  were  between  ±  position  tolerances,  otherwise  set 
fpslop  to  false. 


Name 

Field 

Type 

Range 

Initial 

Log 

Comment 

fpslop 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

all  joints  within  position  error 

fpOslop 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  0  position  slop  flag 

fplslop 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  1  position  slop  flag 

fp2slop 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  2  position  slop  flag 

pOslop 

set  point 

double 

>0.0 

2.6 

c 

joint  0  allowable  error 

plslop 

set  point 

double 

>0.0 

1.8 

c 

joint  1  allowable  error 

p2slop 

set  point 

double 

>0.0 

6.6 

c 

joint  2  allowable  error 

Logic:  if(  ABS(pO  -  pOsp)  <  pOslop)  fpOslop  =  TRUE; 

else  fpOslop  =  FALSE; 

if(  ABS(pl  -  plsp)  <  plslop)  fplslop  =  TRUE; 

else  fplslop  =  FALSE; 

if(  ABS(p2  -  p2sp)  <  p2slop)  fp2slop  =  TRUE; 

else  fp2slop  =  FALSE; 

if(  fpOslop  &&  fplslop  &&  fp2slop)  fpslop  =  TRUE; 
 else    fpslop  =  FALSE; 


Figure  D.2  Joints  0, 1,  and  2  position  error  model. 


131 


132 


Model  Name:  Joints  0, 1,  and  2  velocity 

Function:        Determined  when  joint  0's  velocity  magnitude  was  less  than  allowable  limit. 

Set  fvOslop  to  true  if  velocity  of  joint  0  (vO)  was  between  ±  velocity  tolerance 
(vOslop)  of  zero  (0)  velocity,  otherwise  set  fvOslop  to  false. 

Determined  when  joint  l's  velocity  magnitude  was  less  than  allowable  limit. 
Set  fvOslop  to  true  if  velocity  of  joint  1  (vl)  was  between  ±  velocity  tolerance 
(vlslop)  of  zero  (0)  velocity,  otherwise  set  fvlslop  to  false. 

Determined  when  joint  2's  velocity  magnitude  was  less  than  allowable  limit. 
Set  fv2slop  to  true  if  velocity  of  joint  2  (v2)  was  between  ±  velocity  tolerance 
(v2slop)  of  zero  (0)  velocity,  otherwise  set  fv2slop  to  false. 

Determined  when  all  three  joints  were  within  velocity  limits.  Set  fvslop  to 
true  if  all  joint  velocities  were  between  ±  velocity  tolerances,  otherwise  set 
fvslop  to  false. 


Name 

Field 

Type 

Range 

Initial 

Log 

Comment 

fvslop 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

all  joints  within  velocity  error 

fvOslop 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  0  velocity  slop  flag 

fvlslop 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  1  velocity  slop  flag 

fv2slop 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  2  velocity  slop  flag 

vOslop 

set  point 

double 

>0.0 

48.8 

c 

joint  0  velocity  slop  limit 

vlslop 

set  point 

double 

>0.0 

32.6 

c 

joint  1  velocity  slop  limit 

v2slop 

set  point 

double 

>0.0 

7.0 

c 

joint  2  velocity  slop  limit 

Logic: 

if(  ABS(  vO  )  <  vOslop  ) 

fvOslop  = 

=  TRUE; 

else 

fvOslop  = 

=  FALSE; 

if(  ABS(  vl  )  <  vlslop  ) 
else 

if(  ABS(  v2  )  <  v2slop  ) 
else 


if(  fvOslop  && 
else 


fvlslop  =  TRUE; 
fvlslop  =  FALSE; 

fv2slop  =  TRUE; 
fv2slop  =  FALSE; 

fvlslop  &&  fv2slop  )  fvslop  =  TRUE; 

fvslop  =  FALSE; 


Figure  D.3  Joints  0, 1 ,  and  2  velocity  model. 


Model  Name:  Alignment 

Function: 

Determined  when  the  magnitude  of  both  horizontal  and  vertical  centroid 

errors  were  below  an  allowable  limit.  Set  fvislop  to  true  if  both  magnitudes  of 

centroid  errors  were  below  centroid  tolerances,  otherwise  set  fvislop  to  false. 

Name 

Field 

Type 

Range 

Initial 

Log 

Comment 

fvislop 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

centroid  slop  flag 

viOslop 

set  point 

long  int 

>0 

50 

c 

vertical  error  limit  (pixels) 

vilslop 

set  point 

long  int 

>0 

35 

c 

horizontal  error  limit  (pixels) 

Logic:  if(fblob  &&  (ABS(vilsp  -  xcent)  <  vilslop)  &&  (ABS(vi0sp  -  ycent)  <  viOslop)) 

fvislop  =  TRUE; 

else      fvislop  =  FALSE; 

Figure  D.4  Alignment  model. 
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Model  Name:  Joint  0, 1,  and  2  out-of-limit 

Function:        Determined  if  position  of  joints  0, 1,  or  2  (pO,  pi,  or  p2)  were  outside  maximum 
or  minimum  position  limits.  Set  fpOmax,  fplmax,  or  fp2max  to  true  if  position 
of  joint  was  greater  than  joint's  maximum  position  (pOmax,  plmax,  or  p2max), 
otherwise  set  to  false.  Set  fpOmin,  fplmin,  or  fp2min  to  true  if  position  of  joint 
was  less  than  joint's  minimum  position  (pOmin,  plmin,  or  p2min),  otherwise  set 
to  false.  Set  fpOool,  fplool,  or  fp2ool  to  true  if  joint  position  was  outside  of 
minimum  or  maximum  position  limits,  otherwise  set  to  false  if  joint  position 
was  within  both  maximum  and  minimum  position  limits.  Combined  results 

(fpool)  for  all  three  joints. 

Ma  mo 

r  i  c  i  u 

Type 

ixange 

T  n  i  f  \  a  1 

T  ncr 
LOg 

f"Yvmm  f*n  f 

U  111  1  1L.I  11 

fpool 

result 

flag 

1KUC  II  rALSt 

PA  I  CP 

1 
1 

one  joint  is  out  or  position  limit 

fpOmax 

result 

flag 

TDI  TT7  II  CAT  CT7 

IKUh  II  rALbfc 

PAT  CP 
rALSt 

1 
1 

joint  0  position  above  maximum 

fpOmin 

result 

flag 

TDI  TT7  II  T?  A  T  CC 

1KUL  II  rALbh 

P  A  I  CP 
rALSE 

l 
1 

joint  0  position  below  minimum 

tpUool 

result 

flag 

TRUE  II  FALSE 

rALbfc, 

1 
1 

joint  0  position  out  of  limit 

fplmax 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  1  position  above  maximum 

fplmin 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  1  position  below  minimum 

fplool 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  1  position  out  of  limit 

fp2max 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  2  position  above  maximum 

fp2min 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  2  position  below  minimum 

fp2ool 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

joint  2  position  out  of  limit 

pOmax 

set  point 

double 

31.0 

C 

joint  0  maximum  position  (deg) 

pOmin 

set  point 

double 

-31.0 

c 

joint  0  minimum  position  (deg) 

plmax 

set  point 

double 

15.6 

c 

joint  1  maximum  position  (deg) 

plmin 

set  point 

double 

-15.6 

c 

joint  1  minimum  position  (deg) 

p2max 

set  point 

double 

107.1 

c 

joint  2  maximum  position  (cm) 

p2min 

set  point 

double 

36.9 

c 

joint  2  minimum  position  (cm) 

Logic: 

if(  pO  >  pOmax  ) 
else 

fpOmax 
fpOmax 

=  TRUE; 
=  FALSE; 

if(  pO  <  pOmin  ) 
else 

if(  fpOmax  II  fpOmin  ) 
else 

fpOmin: 
fpOmin  i 
fpOool = 
fpOool  = 

=  TRUE; 
=  FALSE; 
TRUE; 
FALSE; 

if(  pi  >  plmax  ) 
else 

fplmax 
fplmax 

=  TRUE; 
=  FALSE; 

if(  pi  <  plmin  ) 
else 

fplmin  = 
fplmin  = 

=  TRUE; 
=  FALSE; 

if(  fplmax  II  fplmin  ) 
else 

fplool  = 
fplool  = 

TRUE; 
FALSE; 

if(  p2  >  p2max  ) 
else 

fp2max 
fp2max 

=  TRUE; 
=  FALSE; 

if(  p2  <  p2min  ) 
else 

if(  fp2max  II  fp2min  ) 
else 

fp2min  = 
fp2min  = 
fp>2ool  = 
fp2ool  = 

=  TRUE; 
=  FALSE; 
TRUE; 
FALSE; 

if(  fpOool  II  fplool  II  fp2ool ) 
else 

fpool  =  TRUE; 
fpool  =  FALSE; 

Figure  D.5  Joints  0, 1,  and  2  out-of-limit  model. 
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Model  Name:  Proximity 

Function:        Determined  when  an  object  was  close  to  the  ultrasonic  device.  Set  fprox  to  true 
if  distance  to  object  (distp2)  was  less  than  or  equal  than  distance  set  point 
(distsp),  otherwise  set  fprox  to  false. 

Name 

Field 

Type 

Range 

Initial 

Log 

Comment 

fprox 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

proximity  flag 

distsp 

set  point 

double 

>=  4.0  &&  <=  80.0 

11.0 

c 

distance  set  point 

Logic: 

if(  distp2  <=  distsp  ) 
else 

fprox  =  TRUE; 
fprox  =  FALSE; 

Figure  D.6  Proximity  model. 


Vision  series 

Determined  when  the  vision  input  driver  software  had  located  an  object  for  a 
certain  number  of  times  out  of  the  past  10  control  cycles.  Set  fviser  to  true  if  an 
object  had  been  located  a  designated  number  of  times  (vilen)  out  of  the  past  ten 
(10)  control  cycles,  otherwise  set  fviser  to  false. 


Name 

Field 

Type 

Range 

Initial 

Log 

Comment 

fviser 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

vision  series  flag 

vilen 

set  point 

int 

>0&&<  11 

3 

c 

length  of  vision  series 

blob[10] 

storage 

flag 

FALSE 

local  storage  array 

sum 

storage 

int 

0 

history  sum  of  fblob 

index 

storage 

int 

0 

local  array  index  pointer 

Logic:  sum  =  sum  -  blob[index]  +  fblob; 

blob[index]  =  fblob; 
index  =  index  + 1; 

if(index  ==  10)  index  =  0; 

if(  sum  >=  vilen )  fviser  =  TRUE; 

else  fviser  =  FALSE; 


Model  Name: 
Function: 


Figure  D.7  Vision  series  model. 


Model  Name:  Diameter 

Function:        Determined  when  the  horizontal  or  vertical  diameter  of  an  object  exceeded 
diameter  set  points.  Set  fpickd  to  true  if  either  the  horizontal  diameter 
(xdia)  was  greater  than  or  equal  to  horizontal  picking  diameter  set  point 
(vilpickd)  or  the  vertical  diameter  (ydia)  was  greater  than  or  equal  to 
vertical  picking  diameter  set  point  (viOpickd),  otherwise  set  fpickd  to  false. 


Name 


Field 


Type 


Range 


Initial 


Log 


Comment 


fpickd 


result 


flag 


TRUE  II  FALSE 


FALSE 


diameter  flag 


viOpickd 


set  point 


long  int 


<485&&>0 


200 


y  picking  diameter 


vilpickd  set  point  long  int 


<  385  &&  >  0 


120 


x  picking  diameter 


Logic:  if(  fblob  &&  ((xdia  >=  vilpickd)  II  (ydia  >=  viOpickd)))  fpickd  =  TRUE; 
else  fpickd  =  FALSE; 


Figure  D.8  Diameter  model. 
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Model  Name:  Stuck 

Function:        Determined  when  the  magnitude  of  joint  2's  servo-valve  control  word  exceeded 
an  allowable  limit  for  a  specified  time  interval.  Set  f stuck  if  the  magnitude  of 
the  control  word  was  greater  than  a  control  word  limit  (v2cwchk)  for  a  given 
time  interval  (v2cwtim),  otherwise  set  fstuck  to  false. 


Name 

Field 

Type 

Range 

Initial 

Log 

Comment 

fstuck 

result 

flag 

TRUE  II  FALSE 

FALSE 

1 

stuck  flag 

v2cwchk 

set  point 

int 

>=0 

1000 

c 

control  word  limit 

v2cwtim 

set  point 

int 

>=0 

25 

c 

stuck  time  (1/60  s) 

v2erck 

storage 

int 

0 

time  counter 

Logic:  if(  ABS(v2cw)  >  v2cwchk); 

{     v2erck  =  v2erck  +  1; 

if(v2erck  >  v2cwtim)  fstuck  =  TRUE; 
else  fstuck  =  FALSE; 

} 

else 

{    v2erck  =  0; 

fstuck  =  FALSE; 


Figure  D.9  Stuck  model. 


APPENDIX  E 
STATE  PROGRAMMING  FORM 


State  Name 

Alias  Names 

Comment 

abort_pick 

ap  pOmax 
ap_pOmin 
ap_pl  max 
ap_plmin 
ap_p2max 

a p  .stuck 

Abort  current  picking  activity  by  moving  joint  2  to  home  location  using  position  control  and  maintaining  joints  0 
and  1  at  current  locations.  Test  direction  flags  to  determine  which  direction  joints  0  and  1  should  be  moved  in 
search  for  the  next  object  to  pick. 

approach 

Move  joint  2  outward  using  velocity  control  while  using  vision  control  on  joint  0  and  1  to  maintain  end- 
effector — object  alignment. 

command 

Using  position  feedback  control,  move  all  three  joints  to  home  locations.  Test  status  of  three  system  flags  to 
determine  which  control  sequence  will  be  activated. 

detach 

diameter  detach 
slopdetach 

Use  position  control  to  stop  robot  motion  and  activate  picking  device  to  closed  location  to  grasp  object. 

dowrt_left 

Using  velocity  control,  move  joint  0  downward  and  joint  1  to  the  left  in  search  for  fruit.  Use  position  control  on 
joint  2  and  keep  joint  2  at  current  position. 

downright 

Using  velocity  control,  move  joint  0  downward  and  joint  1  to  the  right  in  search  for  fruit.  Use  position  control 
on  joint  2  and  keep  joint  2  at  current  position. 

drop 

Activate  picking  mechanism  to  open  location  while  maintaining  current  joint  positions  on  joints  0,  1,  and  2. 

dump_go_mid 

Power  down  robot.  Using  position  feedback  control,  move  joints  0,1,  and  2  to  mid  locations.  When  all  the 
positions  of  the  three  joints  are  close  to  the  position  set  points  and  velocities  of  joints  are  low,  activate  the 
wait_user  state. 

dump_pull_in 

Power  down  robot.  Using  position  feedback  control,  move  joint  2  to  mid  location  keeping  joints  0  andl  at  current 
locations.  When  velocity  of  joint  2  is  low  and  position  is  close  to  mid  location,  activate  dump_go_mid  state. 

fruit_check 

Stop  robot  motion  and  determine  if  an  object  can  be  located  in  image.  If  an  object  is  located,  activate  alignment, 
otherwise,  start  search  motions 

go_home 

Move  joints  0, 1,  and  2  to  home  locations  using  position  control.  When  all  three  joints  are  close  to  position  set 
points  and  moving  slowly,  activate  command  state. 

go_mid 

Using  position  feedback  control,  move  joints  0, 1,  and  2  to  mid  locations.  When  all  three  joints  are  close  to  their 
mid  locations,  activate  command  state. 

home_puIl_in 

Using  position  control,  move  joint  2  to  home  location  and  keep  joints  0  and  1  at  current  locations.  When  joint  2 
is  moving  slowly  and  either  position  set  point  has  been  reached  or  two  seconds  elapse,  activate  go  home  state. 

joy_stick 

Activate  joy  stick  feedback  on  joints  0, 1,  and  2.  Activate  home_pull_in  state  if  joy  stick  flag  is  set  to  false. 

leave  pick 

Using  velocity  control,  move  joint  2  to  home  location.  Using  position  control,  keep  joints  0  and  1  at  current 
locations.  When  joint  2  reaches  position  set  point,  activate  command  state. 

lip_down 

Keep  joints  0, 1,  and  2  at  current  locations  and  activate  picking  device  to  open  location. 

lip_up 

Keep  joints  0, 1,  and  2  at  current  locations  and  activate  picking  device  to  closed  location. 

occluded 

Move  joint  2  out  slightly  while  vision  servoing  on  object. 

pick_pull_in 

Using  position  control  on  all  three  joints,  move  joint  2  to  home  location  while  keeping  joints  0  and  1  at  current 
locations.  Activate  fruit_check  state  when  joint  2  is  close  to  position  set  point. 

point 

Use  vision  feedback  on  joints  0  and  1  to  align  end-effector  on  object  located  in  image.  Maintain  joint  2  at  current 
position. 

pull_in 

Using  position  feedback  control,  move  joint  2  to  mid  location  keeping  joints  0  and  1  at  current  locations.  When 
joint  2  is  close  to  mid  location  and  moving  slowly,  activate  go_mid  state. 

release 

Move  joint  2  to  home  location  using  velocity  feedback  keeping  joints  0  and  1  at  current  locations. 

retract 

Move  joint  2  to  home  location  using  velocity  feedback  keeping  joints  0  and  1  at  current  locations. 

slowout 

Use  vision  feedback  on  joints  0  and  1  to  align  end-effector,  extend  joint  2  using  velocity  control  at  a  slow  outward 
velocity. 

start_up 

State  activated  at  system  start  up.  Establish  position  control  on  joints  0, 1,  and  2  using  current  positions  for 
position  set  points.  Activate  pull  Jn  state  when  hydraulic  power  flag  is  set  to  true. 

touch 

Dead  reckoning  final  approach  to  object.  Maintain  position  of  joints  0  and  1  while  using  position  control  on  joint 
2  to  slowly  move  out  the  picking  distance. 

track 

Establish  vision  control  on  joints  0  and  1  to  align  end-effector  with  object  located  by  vision  input  driver  software. 
Use  position  control  on  joint  2  to  maintain  joint  position  at  current  location. 

upjeft 

Using  velocity  control,  move  joint  0  upward  and  joint  1  to  the  left  in  search  for  fruit.  Use  position  control  on 
joint  2  and  keep  joint  2  at  current  position. 

U  p  r  i  g  ht 

Using  velocity  control,  move  joint  0  upward  and  joint  1  to  the  right  in  search  for  fruit  Use  position  control  on 
joint  2  and  keep  joint  2  at  current  position. 

vision_touch 

Use  vision  control  on  joints  0  and  1  for  final  approach  to  object. 

wait_pick 

Used  when  object  is  lost  by  vision  driver  software.  Move  joint  2  outward  for  a  closer  look  at  canopy. 

wait_velocity 

Use  vision  control  on  joints  0  and  1  to  maintain  end-effector— object  alignment  but  stop  outward  movement  of 
joint  2  until  velocities  of  joints  0  and  1  are  reduced. 
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APPENDIX  F 
STATE  NETWORK  MAP 


Initialization 

abort_pick 

approach 

command 

detach 

down_left 

down_right 

jointOctl 

P 

VI 

P 

P 

V 

V 

jointlctl 

P 

VI 

P 

P 

V 

V 

joint2ctl 

V 

V 

P 

P 

p 

p 

pOsp 

pO 

pOhome 

po 

pOdn 

pOdn 

plap 

Pi 

pi  home 

Pi 

pi  If 

plrt 

p2sp 

p2home 

p2home 

P2 

p2home 

p2home 

vOsp 

vOdn 

vOdn 

vlsp 

vl  If 

vlrt 

v2sp 

v2ret 

v2fast 

viOsp 

vlOmid 

vilsp 

vilmid 

fhydpwr 

ON 

ON 

ON 

ON 

ON 

ON 

flights 

OFF 

ON 

OFF 

OFF 

ON 

ON 

pdevice 

OPEN 

OPEN 

OPEN 

CI-OSED 

OPEN 

OPEN 

fdata 

dttime 

pmdelay 

wait 

wait 

fdiiO 

DOWN 

DOWN 

fdirl 

LEFT 

RIGHT 

fjoy 

FALSE 

fpick 

FALSE 

fpowerup 

ftrack 

FALSE 

occdist 

wait 

waltdef 

0 

0 

distsp 

pOslop 

pi  slop 

p2slop 

vOslop 

vlslop 

v2slop 

viOslop 

vilslop 

viOpickd 

vllpickd 

v2cwchk 

600 

State  Links 

abort_pick 

approach 

command 

detach 

down_left 

down_right 

abort  pick 

ap  pOmax 

2.  fpOmax 

2.  fpOmax 

ap_p0min 

3.  fpOmin 

3.  fpOmin 

ap  pi  max 

4.  fpl  max 

4.  fplmax 

ap_plmin 

5.  fplmin 

5.  fplmin 

ap_p2max 

6.  fp2max 

ap_stuck 

7.  fstuck 

approach 

command 

detach 

diameter  detach 

down  left 

5.  fp2slop  A&  IfdirO  &A  Ifdirl 

4.  fpl  slop  II  fplmax 

down_right 

4.  fp2slop  &&  IfdirO  &*  fdirl 

3.  fpl  slop  H  fplmin 

drop 

dump^go_mid 

dump_pull_in 

fruit  check 

gohome 

go  mid 

home_pull_in 

joy_stick 

3.  fjoy 

leave_pick 

1.  Ifpick 

1.  Ifpick 

1.  Ifpick 

1.  Ifpick 

1.  Ifpick 

lip  down 

lip  up 

occluded 

12.fprox  &&  !fpickd 

pick_pull_in 

1.  fpick 

point 

2.  fdt  &&  fviser 

2.  fdt  &&  fviser 

pull_in 

release 

retract 

6.  fdt 

slop_detach 

slow  out 

9.  Ifvislop 

start_up 

touch 

track 

2.  ftrack 

upjeft 

3.  fp2slop  &&  fdirO  &&  Ifdirl 

4fp0slopllfp0max 

up_right 

2.  fp2slop  &&  fdirO  &&  fdirl 

3.  fpOslop  II  fpOmax 

vision  touch 

ll.fproxi&fpickd 

wait_pick 

8.  !fviser 

wait_velocity 

10.  IfvOslop  II  Ifvislop 
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Initialization 

drop 

dumpgomid 

dump_pull_in 

fruit_check 

go_home 

jointOctl 

P 

P 

P 

P 

P 

jointlctl 

P 

P 

P 

P 

P 

joint2ctl 

P 

P 

P 

P 

P 

Pfep 

PO 

pOmid 

I* 

pOhome 

pl«p 

Pi 

pi  mid 

pl 

pi 

plhome 

p2sp 

p2home 

p2mid 

p2mid 

p2home 

p2home 

vOsp 

vlsp 

v2sp 

viOsp 

vilsp 

fhydpwr 

ON 

OFF 

OFF 

ON 

ON 

flights 

OFF 

OFF 

ON 

OFF 

pdevice 

OPEN 

OFF 

OFF 

OPEN 

OPEN 

[data 

dttime 

pmdelay 

180 

240 

20 

WliO 

fdirl 

fjoy 

fpick 

fpowerup 

OFF 

OFF 

ftrack 

occdist 

wait 

distsp 

pOslop 

26 

Z6 

pi  slop 

LI 

|2 

p2slop 

44 

U 

AA 

vOslop 

U 

Ii 

vlslop 

u 

ii 

v2slop 

07 

07 

0.7 

viOslop 

vilslop 

viOpickd 

vilpickd 

v2cwchk 

State  links 

drop 

dumpgomid 

dump_pulMn 

fruit_check 

go_home 

abort_pick 

3.  fdt  &&  tfviser 

ap_p0max 

ap^pOmin 

ap_plmax 

ap_plmin 

ap_p2max 

ap  stuck 

approach 

command 

1.  fpslop &&  fvslop 

detach 

diameter  detach 

down  left 

down_right 

drop 

dump^go^mid 

l.fv2slop&£(fp2slop  II  fdt) 

dump_pull_in 

fruit  check 

go_home 

gomid 

home_pulMn 

joy_stick 

leave_pick 

L  !fpick 

1.  !fpick 

lip  down 

Hp_up 

2.fdt 

occluded 

pick_pull_in 

point 

2.  fdt  fviser 

pull.ln 

release 

retract 

slop_detach 

slow  out 

start_up 

1 .  fvslop  Sl&  (fpslop  II  fdt) 

touch 

track 

up_left 

up_right 

vision  touch 

wait^pick 

wait_velocity 
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Initialization 

go  mid 

home_pull_in 

joy_stick 

leave_pick 

lip_down 

lip_up 

occluded 

jointOctl 

P 

P 

J 

P 

P 

P 

VI 

jointlctl 

P 

P 

J 

P 

P 

P 

VI 

jolnt2ctl 

P 

P 

J 

V 

P 

P 

V 

pfep 

pOmid 

po 

P0 

P° 

pO 

plsp 

pi  mid 

Pi 

Pi 

Pi 

Pi 

p2sp 

p2mid 

p2home 

p2home 

p2home 

p2home 

p2  +  occdist 

vOsp 

0 

vlsp 

0 

v2sp 

0 

v2ret 

v2slow 

viOsp 

viOmid 

vilsp 

vilmid 

fhydpwr 

ON 

ON 

ON 

ON 

ON 

ON 

ON 

flights 

OFF 

OFF 

OFF 

OFF 

OFF 

OFF 

ON 

pdevice 

OFF 

OPEN 

OPEN 

OPEN 

OPEN 

CIjOSED 

OPEN 

fdata 

dttime 

120 

pmdelay 

pmdelay 

fdirO 

fdirl 

fjoy 

fpick 

fpowerup 

ftrack 

ocodist 

wait 

distsp 

pOslop 

26 

pi  slop 

u 

p2slop 

4.4 

44 

vOslop 

u 

vlslop 

u 

v2slop 

0.7 

0.7 

viOslop 

vilslop 

vlOplckd 

viOppd 

vilpickd 

vilppd 

v2cwchk 

500 

State  links 

go_mid 

home_pull_in 

joy_stick 

leave_pick 

lip_down 

Hp_up 

occluded 

abort  _pick 

ap_p0max 

2.  fpOmax 

ap_p0min 

3.  fpOmin 

ap_pl  max 

4.  fplmax 

ap_plmin 

5.  fplmin 

ap_p2max 

6.  fp2max 

a p  stuck 

7.  fstuck 

approach 

1 1.  Ifprox  fviser 

command 

1.  fpslop  SlSl  fvslop 

1 .  fp2slop 

detach 

diameter_detach 

9.  fpkkd 

down_left 

down_right 

drop 

dumpgomid 

dump_pull_ln 

fruit  check 

go_home 

I.fv2slop  &&(fp2slopll  fdt) 

go_mid 

home_pull__in 

l.ifjoy 

joystick 

leave_pick 

1.  Ifpick 

1.  Ifpick 

1.  ifpick 

Hp_down 

2.  fdt 

lip_up 

occluded 

pick_pull_in 

2.  fdt 

point 

pull_in 

release 

2.  fstuck 

retract 

sloped  etach 

10.  fp2slop 

slow  out 

start  up 

touch 

8.  !fviser 

track 

up_left 

up__right 

vision  touch 

wait_pick 

w  a  it  _  velocity 

1 
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TniHaliratinn 

pick  pull  in 

po  n 

ull  in 
pu  _  n 

release 

retract 

slow  out 

jointOctl 

p 

VI 

P 

P 

P 

VI 

joint!  ctl 

p 

VI 

P 

P 

P 

VI 

jolnt2ctl 

p 

p 

P 

P 

V 

V 

pOrp 

P° 

PO 

PO 

PO 

plsp 

pi 

P1 

P1 

pi 

P2»P 

p2home 

P2 

p2mid 

P2 

p2home 

vOsp 

vlsp 

v2sp 

v2ret 

v2slow 

viOsp 

viOmid 

viOmid 

vilsp 

vilmid 

vilmid 

fhydpwr 

ON 

ON 

ON 

ON 

ON 

ON 

flights 

OFF 

ON 

OFF 

OFF 

OFF 

ON 

pdevice 

OPEN 

OPEN 

o\v 

OPEN 

CLOSED 

OPEN 

fdata 

ON 

dttime 

60 

120 

pmdelay 

fdirO 

fdirl 

fjoy 

fpick 

fpowerup 

ftrack 

ocodist 

wait 

distsp 

pOslop 

pi  slop 

p2s!op 

44 

vOsiop 

vlslop 

v2slop 

0.7 

viOslop 

70 

vtl  slop 

SO 

viOpickd 

vilpickd 

v2cwchk 

350 

State  links 

pick.  pul)_in 

point 

pull_in 

release 

retract 

slow_out 

abort  pick 

2.  fdt 

ap_p0max 

2.fp0max 

2.  fpOmax 

ap_p0min 

3.  fpOmin 

3.  fpOmin 

ap_plmax 

4.  fplmax 

4.  fplmax 

ap_plmin 

5.  fplmin 

5.  fpl min 

ap_p2max 

6.  fp2max 

a p  stuck 

7.  fstuck 

approach 

6.  fviser  &&  fvislop 

8.  fvislop 

command 

detach 

diameter  detach 

down  left 

down_right 

d  rop 

3.  fp2slop 

dump_go_mid 

dump_pull_in 

l.fdt  &&  Ipsdmp 

fruit  check 

2.  fpslop 

go_home 

go_mid 

2.  fv2slop  &&  (fP2slop  II  fdt) 

home_pull_in 

joy_stick 

leave^pick 

1.  Ifpick 

1.  Ifpick 

1.  Jfpick 

1.  Ifpick 

1.  ifpick 

lip_down 

iip  up 

occluded 

12.  fprox  &&  ifpickd 

pick_pull_in 

point 

pull_in 

release 

3.  fstuck 

2.  fstuck 

ret  ract 

slop_detach 

slow  out 

7.  fviser  &&  fdt 

start_up 

touch 

track 

upjeft 

up_right 

vision_touch 

11.  fprox  &&  fpickd 

wait_pick 

8.  Ifviser 

9.  ifviser 

wait^velodty 

1 

10.  IfvOslop  II  Ifvlslop 
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initialization 

a  _up 

touch 

track 

left 

up  right 
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